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

using System.Collections.Generic;
using Controller.Bicycle;
using UnityEngine;
using UnityEngine.Serialization;
using Valve.VR.InteractionSystem;

namespace Controller.Bicycle
{
    public class WcBicycleController : MonoBehaviour, IBicycleController
    {
        #region Variables

        [Header("Visible Game Objects")] public GameObject rearWheel;
        public GameObject frontWheel;

        [Header("Game Objects for Physics")] public Transform centerOfMass;
        [FormerlySerializedAs("rb")] public Rigidbody rigidBody;
        public WheelConfig wheelConfig;

        [Header("Values")] public float offsetCollidersFromWheel = 0.25f;
        public float maxLeaningAngle = 35f;
        public float maxSteeringAngle = 80f;
        [Range(0,15)]
        [Tooltip("Speed in m/s")]
        public float maxSpeed = 11.111f; //40 km/h
        public float maxBreakTorque = 2000f;
        public float maxMotorTorque = 1000f;

        public BicycleControllerMode ControllerMode { get; set; }

        public float CurrentSpeed
        {
            get => currentSpeed;
            set => currentSpeed = Mathf.Clamp(value, 0, maxSpeed);
        }

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

        public float CurrentLeaningAngle
        {
            get => currentLeaningAngle;
            set => currentLeaningAngle = Mathf.Clamp(value, -maxLeaningAngle, maxLeaningAngle);
        }

        private WheelCollider[] allWheelColliders;
        private float initialWheelColliderY;
        private float currentSteerAngle = 0f;
        private float currentLeaningAngle = 0f;
        private float currentSpeed;

        #endregion

        // Start is called before the first frame update
        void Start()
        {
            rigidBody.centerOfMass = centerOfMass.localPosition;
            allWheelColliders = wheelConfig.AllWheels;
            wheelConfig.AdjustToGameObjects(frontWheel.transform, rearWheel.transform, offsetCollidersFromWheel);
            initialWheelColliderY = allWheelColliders[0].transform.localPosition.y;
        }


        void FixedUpdate()
        {
            ApplyColliderForces();
            Lean();
            //RotateStraight();
        }

        private void ApplyColliderForces()
        {
            //offsetCollidersFromWheel = Mathf.Max(0.05f, 0.45f - rigidBody.velocity.magnitude * 0.07f);
            //wheelConfig.AdjustToGameObjects(frontWheel.transform, rearWheel.transform, offsetCollidersFromWheel);
            ControlSteer(new[] {wheelConfig.frontLeft, wheelConfig.frontRight});
            ControlTorque(new[] {wheelConfig.rearLeft, wheelConfig.rearRight});
        }

        private void ControlSteer(IEnumerable<WheelCollider> colliders)
        {
            //float steering = maxSteeringAngle * CurrentSteerAngle * 0.2f;
            //leftWheels.localPosition = -Vector3.up * (relativeLeanAmount * CurrentSteerAngle * rigidBody.velocity.magnitude * 0.2f);
            //rightWheels.localPosition = Vector3.up * (relativeLeanAmount * CurrentSteerAngle * rigidBody.velocity.magnitude * 0.2f);
            colliders.ForEach(c => c.steerAngle = CurrentSteerAngle);
        }

        private void ControlTorque(IEnumerable<WheelCollider> colliders)
        {
            var rbSpeed = rigidBody.velocity.magnitude;
            var speedDif = CurrentSpeed - rbSpeed;
            var ratio = speedDif / maxSpeed;
           

            var brakeTorque = 0f;
            var motorTorque = 0f;
            
            if (speedDif >= .1f) // 0.36 km/h
            {
                var torque = maxMotorTorque * ratio;
                //Debug.Log($"SpeedDif = {speedDif} -> applying Torque {torque} (Ratio: {ratio})");
                brakeTorque = 0;
                motorTorque = torque;
            }
            else if (speedDif <= -.1f)
            {
                var torque = -maxBreakTorque * ratio;
                //Debug.Log($"SpeedDif = {speedDif} -> applying brake Torque {torque} (Ratio: {ratio})");
                motorTorque = 0;
                brakeTorque = torque;
            }

            foreach (var c in colliders)
            {
                c.motorTorque = motorTorque;
                c.brakeTorque = brakeTorque;
            }
        }

        private void Lean()
        {
            //reset all wheels to being centered
            if (CurrentLeaningAngle == 0) //TODO: maybe add a threshold for leaning, e.g. < -0.05 and > 0.05 
            {
                //leaning left, left wheels up, right wheels down
                ApplyOffsetToTransform(wheelConfig.frontLeft.transform, initialWheelColliderY);
                ApplyOffsetToTransform(wheelConfig.rearLeft.transform, initialWheelColliderY);
                ApplyOffsetToTransform(wheelConfig.frontRight.transform, initialWheelColliderY);
                ApplyOffsetToTransform(wheelConfig.rearRight.transform, initialWheelColliderY);
            }

            //CurrentLeaningAngle < 0 -> leaning left, > 0 -> right
            var leaningAbs = Mathf.Abs(CurrentLeaningAngle * Mathf.Deg2Rad);

            //calculate offset for wheels; description Docu folder
            //1.57079633 rad = 90 deg
            var verticalOffset = offsetCollidersFromWheel * Mathf.Sin(leaningAbs) / Mathf.Sin(1.57079633f - leaningAbs);
            var yPlusOffset = initialWheelColliderY + verticalOffset;
            var yMinusOffset = initialWheelColliderY - verticalOffset;

            if (CurrentLeaningAngle < 0) //TODO: maybe add a threshold for leaning, e.g. < -0.05 and > 0.05 
            {
                //leaning left, left wheels up, right wheels down
                ApplyOffsetToTransform(wheelConfig.frontLeft.transform, yPlusOffset);
                ApplyOffsetToTransform(wheelConfig.rearLeft.transform, yPlusOffset);
                ApplyOffsetToTransform(wheelConfig.frontRight.transform, yMinusOffset);
                ApplyOffsetToTransform(wheelConfig.rearRight.transform, yMinusOffset);
            }
            else if (CurrentLeaningAngle > 0)
            {
                //leaning right, right wheels up, left wheels down
                ApplyOffsetToTransform(wheelConfig.frontLeft.transform, yMinusOffset);
                ApplyOffsetToTransform(wheelConfig.rearLeft.transform, yMinusOffset);
                ApplyOffsetToTransform(wheelConfig.frontRight.transform, yPlusOffset);
                ApplyOffsetToTransform(wheelConfig.rearRight.transform, yPlusOffset);
            }
        }

        private void ApplyOffsetToTransform(Transform t, float newY)
        {
            var oldPos = t.localPosition;
            t.localPosition = new Vector3(oldPos.x, newY, oldPos.z);
        }
    }
}