VRCycling/Assets/Scripts/Wheels/LerpSlopeCollider.cs

using System;
using Roads;
using UnityEngine;

namespace Wheels
{
    public class LerpSlopeCollider : MonoBehaviour
    {
        public int collisionLayer = 1;

        public Transform rearWheelContact;
        public Transform frontWheelContact;
        public Rigidbody bike;

        private const float TOLERANCE = 0.005f;
        private const float MAX_RAYCAST_LENGTH = 2f;

        private float distanceUntilCompletelyRotated;
        private Transform bikeTransform;
        private float currentFrontWheelSlope;
        private float currentFrontWheelMinY;
        private bool rotate;
        private int slopeDirection = 1;

        private void Start()
        {
            bikeTransform = bike.transform;
            distanceUntilCompletelyRotated = Vector3.Distance(frontWheelContact.position, rearWheelContact.position);
        }

        private void OnGUI()
        {
            GUI.TextField(new Rect(10, 200, 200, 20), $"Direction = {slopeDirection}");
            GUI.TextField(new Rect(10, 220, 200, 20), $"MinY = {currentFrontWheelMinY}");
            GUI.TextField(new Rect(10, 250, 200, 20), $"rotation.x = {bikeTransform.localRotation.eulerAngles.x:n2}");
            GUI.TextField(new Rect(10, 270, 200, 20), $"Slope = {currentFrontWheelSlope}");
        }

        private void FixedUpdate()
        {
            var fwHit = CastRay(frontWheelContact.position, Vector3.down);

            if (fwHit != null)
            {
                if (Mathf.Abs(currentFrontWheelSlope - fwHit.Slope) > TOLERANCE)
                {
                    rotate = true; //begin rotation
                    currentFrontWheelSlope = fwHit.Slope;
                }

                var minYDif = currentFrontWheelMinY - fwHit.MinY;
                if (Mathf.Abs(minYDif) > TOLERANCE)
                {
                    slopeDirection = -Math.Sign(minYDif);
                    currentFrontWheelMinY = fwHit.MinY;
                }
            }

            RotateIfNeeded();
        }

        private IRoad CastRay(Vector3 pos, Vector3 direction)
        {
            var layerMask = 1 << collisionLayer;

            if (Physics.Raycast(pos, direction, out var hit, MAX_RAYCAST_LENGTH, layerMask))
            {
                Debug.DrawRay(pos, direction * hit.distance, Color.green);
                var roadHit = hit.collider.gameObject.GetComponentInParent<IRoad>();
                return roadHit;
            }

            Debug.DrawRay(pos, direction * MAX_RAYCAST_LENGTH, Color.white);
            return null;
        }

        private void RotateIfNeeded()
        {
            if (rotate)
            {
                var bikeAngle = bikeTransform.localRotation.eulerAngles.x;
                if (bikeAngle < -180)
                {
                    bikeAngle = Mathf.Abs(360 + bikeAngle);
                }
                else if (bikeAngle > 180)
                {
                    bikeAngle = Mathf.Abs(-360 + bikeAngle);
                }

                var slopeDif = slopeDirection * (currentFrontWheelSlope - bikeAngle);
                if (Mathf.Abs(currentFrontWheelSlope - bikeAngle) <
                    TOLERANCE)
                {
                    rotate = false;
                    return;
                }

                var forwardBikeSpeed = bikeTransform.InverseTransformDirection(bike.velocity).z;
                var rot = slopeDif * Time.fixedDeltaTime * forwardBikeSpeed / distanceUntilCompletelyRotated;

                if (slopeDif > 0) //lift front wheel
                {
                    bikeTransform.RotateAround(frontWheelContact.position, -frontWheelContact.right,
                        rot);
                }
                else //dip front wheel
                {
                    bikeTransform.RotateAround(frontWheelContact.position, frontWheelContact.right,
                        -rot);
                }
            }
        }
    }
}