VRCycling/Assets/Scripts/SlopeCollider.cs

using System;
using UnityEngine;

public enum SlopeDirection
{
    Level,
    Uphill,
    Downhill
}

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

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

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

public class SlopeCollider : MonoBehaviour
{
    public int collisionLayer = 1;
    public Transform rearWheelContactPoint;
    public Transform frontWheelContactPoint;

    private const float THRESHOLD = 0f;
    private Transform t;
    private float distRwFw;
    private SlopeDirection currentSlope = SlopeDirection.Level;

    private void Start()
    {
        t = 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?.even ?? true) && (rwHit?.even ?? true)) return;

        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);
            }*/
        }

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

    private void Rotate(float dist, float distDif, Vector3 fwContact, Vector3 rwContact)
    {
        var angle = Mathf.Atan(dist / distRwFw) * Mathf.Rad2Deg;

        if (distDif < -THRESHOLD) //rwDist > fwDist
        {
            //rear wheel in the air -> rotate around front wheel -> make it go uphill
            currentSlope = SlopeDirection.Uphill;
            t.RotateAround(fwContact, frontWheelContactPoint.right, -angle);
        }
        else if (dist > THRESHOLD)
        {
            //front wheel in the air -> rotate around rear wheel
            currentSlope = SlopeDirection.Downhill;
            t.RotateAround(rwContact, rearWheelContactPoint.right, angle);
        }
    }

    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, -t.up, out hit, 20f, layerMask))
        {
            Debug.DrawRay(start, -t.up * hit.distance, Color.green);
            Debug.DrawRay(hit.point, hit.normal, Color.blue);
            var isUneven = hit.collider.gameObject.CompareTag(StreetPartMetaTag.TAG_UNEVEN);
            var first = -(-t.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);
        }

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