HoHCI-BikeSim/Assets/Scripts/SlopeCollider.cs

using System;
using UnityEngine;

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;
    }
}