VRCycling/Assets/Scripts/Wheels/SlopeCollider.cs

using System;
using UnityEngine;

namespace Wheels
{
    public enum RotateDirection
    {
        Unknown,
        None,
        Forward,
        Backward
    }

    internal struct SlopeHit
    {
        internal float distance;
        internal bool even;
        internal float angle;
        internal Vector3 hitPoint;

        internal SlopeHit(float distance, bool even, float angle, Vector3 hitPoint)
        {
            this.distance = distance;
            this.even = even;
            this.angle = angle;
            this.hitPoint = hitPoint;
        }

        public override string ToString()
        {
            return $"SlopeHit(distance = {distance}, even = {even}, angle = {angle})";
        }
    }

    public class SlopeCollider : MonoBehaviour
    {
        public int collisionLayer = 1;
        public SphereCollider rearWheelCollider;
        public SphereCollider frontWheelCollider;
        public Transform bike;

        private const float THRESHOLD = 0.003f;
        private float distRwFw;
        private RotateDirection currentRotateDirection = RotateDirection.Unknown;
        private Transform frontWheelContactPoint;
        private Transform rearWheelContactPoint;

        private void Start()
        {
            frontWheelContactPoint = frontWheelCollider.transform;
            rearWheelContactPoint = rearWheelCollider.transform;
            distRwFw = Mathf.Abs(frontWheelContactPoint.localPosition.z - rearWheelContactPoint.localPosition.z);
        }

        private void FixedUpdate()
        {
            var fwContact = frontWheelContactPoint.position;
            var rwContact = rearWheelContactPoint.position;
            var fwHit = DrawRay(fwContact);
            var rwHit = DrawRay(rwContact);

            if (!fwHit.HasValue || !rwHit.HasValue)
            {
                return;
            }

            //Debug.Log("----Slope Collider----");
            //Debug.Log($"\tfwHit: {fwHit}");
            //Debug.Log($"\trwHit: {rwHit}");

            var fw = fwHit.Value;
            var rw = rwHit.Value;
            var fwDist = fw.distance;
            var rwDist = rw.distance;
            var distDif = fwDist - rwDist;

            if (fwDist >= THRESHOLD && rwDist >= THRESHOLD)
            {
                var deltaY = fwDist <= rwDist ? fwDist : rwDist;
                bike.Translate(0,-deltaY,0);
                //Debug.Log($"Translated {-deltaY} on y");
            }
            else if (fw.distance >= THRESHOLD)
            {
                var angle = Mathf.Atan(fw.distance / distRwFw) * Mathf.Rad2Deg;
                //Debug.Log($"Rotating {angle} deg");
                bike.RotateAround(rearWheelContactPoint.position, rearWheelContactPoint.right, angle);
            }
            else if (rw.distance >= THRESHOLD)
            {
                var angle = Mathf.Atan(rw.distance / distRwFw) * Mathf.Rad2Deg;
                bike.RotateAround(frontWheelContactPoint.position, frontWheelContactPoint.right, -angle);
                //Debug.Log($"Rotating {-angle} deg");
            }

            //if ((fwHit?.even ?? true) && (rwHit?.even ?? true)) return;

            /*var distFw = fwHit?.distance ?? 0f;
        var distRw = rwHit?.distance ?? 0f;
        var distDif = distFw - distRw;
        
        if (fwHit.HasValue && rwHit.HasValue)
        {
            var fw = fwHit.Value;
            var rw = rwHit.Value;
            if (!fw.even && rw.even)
            {
                if (fw.hitPoint.y > rw.hitPoint.y && currentRotateDirection != RotateDirection.Forward)
                {
                    currentRotateDirection = RotateDirection.Backward;
                }else if (currentRotateDirection != RotateDirection.Backward)
                {
                    currentRotateDirection = RotateDirection.Forward;
                }
            }else if (fw.even && !rw.even)
            {
                if (fw.hitPoint.y > rw.hitPoint.y && currentRotateDirection != RotateDirection.Backward)
                {
                    currentRotateDirection = RotateDirection.Forward;
                }
                else if(currentRotateDirection != RotateDirection.Forward)
                {
                    currentRotateDirection = RotateDirection.Backward;
                }
                
            }
            else
            {
                currentRotateDirection = RotateDirection.None; //TODO: maybe not
            }
        }
        else
        {
            if (distDif < -THRESHOLD && currentRotateDirection != RotateDirection.Forward)
            {
                currentRotateDirection = RotateDirection.Backward;
            }else if (distDif > THRESHOLD && currentRotateDirection != RotateDirection.Backward)
            {
                currentRotateDirection = RotateDirection.Forward;
            }
            else
            {
                currentRotateDirection = RotateDirection.None;
            }
        }

        Debug.Log("CurrentRotateDirection = " + currentRotateDirection);
        Debug.Log("----Slope Collider----");
        Debug.Log($"\tfwHit: {fwHit}");
        Debug.Log($"\trwHit: {rwHit}");

        if (currentRotateDirection == RotateDirection.Backward) //rwDist > fwDist
        {
            //rear wheel in the air -> rotate around front wheel -> make it go uphill
            var angle = Mathf.Atan(distRw / distRwFw) * Mathf.Rad2Deg;
            bike.RotateAround(fwContact, frontWheelContactPoint.right, -angle);
        }
        else if (currentRotateDirection == RotateDirection.Forward)
        {
            //front wheel in the air -> rotate around rear wheel
            var angle = Mathf.Atan(distFw / distRwFw) * Mathf.Rad2Deg;
            bike.RotateAround(rwContact, rearWheelContactPoint.right, angle);
        }

        /*if (rwHit.HasValue)
        {
            var rwHitVal = rwHit.Value;
            if (rwHitVal.angle > 0.01f)
            {
                t.RotateAround(fwContact, frontWheelContactPoint.right, -rwHitVal.angle);
            }
            /*if (rwHitVal.even)
            {
                //begin of slope
                Rotate(rwHitVal.distance, rwHitVal.distance - (fwHit?.distance ?? 0f), fwContact, rwContact);
            }
        }

        if (fwHit.HasValue)
        {
            var fwHitVal = fwHit.Value;
            if (fwHitVal.angle > 0.01f)
            {
                t.RotateAround(rwContact, rearWheelContactPoint.right, -fwHitVal.angle);
            }
            /*if (fwHitVal.even)
            {
                //end of slope
                Rotate(fwHitVal.distance, fwHitVal.distance - (rwHit?.distance ?? 0f), fwContact, rwContact);
            }
        }*/
        }


        private SlopeHit? DrawRay(Vector3 start)
        {
            var layerMask = 1 << collisionLayer;
            RaycastHit hit;
            // Does the ray intersect any objects excluding the player layer
            if (Physics.Raycast(start, -bike.up, out hit, 20f, layerMask))
            {
                Debug.DrawRay(start, -bike.up * hit.distance, Color.green);
                //Debug.DrawRay(hit.point, hit.normal, Color.blue);
                var isUneven = hit.collider.gameObject.CompareTag(StreetPartMetaTag.TAG_UNEVEN);
                var first = -(-bike.up * hit.distance);
                var second = (hit.normal);
                var angle = Mathf.Acos(Vector3.Dot(first, second) / first.magnitude * second.magnitude) * Mathf.Rad2Deg;
                //Debug.Log("Dot Product: " + Vector3.Dot(first, second));
                //Debug.Log("Angle: " + angle);
                return new SlopeHit(hit.distance, !isUneven, angle, hit.point);
            }

            if (Physics.Raycast(start, bike.up, out hit, 20f, layerMask))
            {
                Debug.DrawRay(start, -bike.up * hit.distance, Color.green);
                //Debug.DrawRay(hit.point, hit.normal, Color.blue);
                var isUneven = hit.collider.gameObject.CompareTag(StreetPartMetaTag.TAG_UNEVEN);
                var first = -(-bike.up * hit.distance);
                var second = (hit.normal);
                var angle = Mathf.Acos(Vector3.Dot(first, second) / first.magnitude * second.magnitude) * Mathf.Rad2Deg;
                //Debug.Log("Dot Product: " + Vector3.Dot(first, second));
                //Debug.Log("Angle: " + angle);
                return new SlopeHit(-hit.distance, !isUneven, angle, hit.point);
            }

            //Debug.DrawRay(start, -t.up * 20f, Color.red);
            return null;
        }
    }
}