VRCycling/Assets/Scripts/Wheels/ColliderAddSlopeAdjustment.cs

using UnityEngine;

public class ColliderAddSlopeAdjustment : MonoBehaviour
{
    private const float RAYCAST_DIST = 0.2f;

    [Header("Steering Direction Config")]
    [Tooltip(
        "A Transform that has an Axis that always shows into steering direction. It is assumed, that this transform is always on the floor!")]
    public Transform steerTransform;

    public Axis steerTransformForward = Axis.Z;
    public bool backwardIsForward;
    public int collisionLayer = 11;
    private readonly float rayLength = 10f; //TODO: calculate -> front wheel

    private void FixedUpdate()
    {
        var hit = AdjustSlope(); //Check above ground -> ascending
        if (!hit) AdjustSlope(true); //Check underground -> descending
    }

    private bool AdjustSlope(bool underground = false)
    {
        var hitDistLower = CastRay(underground: underground);
        var hitDistUpper = CastRay(RAYCAST_DIST, underground);

        if (hitDistLower != null && hitDistUpper != null)
        {
            var slope = CalculateHitSlope(hitDistLower.Value, hitDistUpper.Value);
            //Debug.Log($"Hit - Slope = {(underground ? -slope : slope )}");
            return true;
        }

        return false;
    }

    //TODO: does only work for going up! We also need rays underground
    private float CalculateHitSlope(float hitDistLower, float hitDistUpper)
    {
        return Mathf.Atan(RAYCAST_DIST / Mathf.Abs(hitDistUpper - hitDistLower)) * Mathf.Rad2Deg;
    }

    private float? CastRay(float dist = 0f, bool underground = false)
    {
        var layerMask = 1 << collisionLayer;
        RaycastHit hit;
        var forward = CalculateForward();
        var position =
            steerTransform.position +
            Vector3.up * (dist - (underground ? 1 : 0)); // TODO: probably needs to be transformed to local axes
        // Does the ray intersect any objects excluding the player layer
        if (Physics.Raycast(position, forward, out hit, rayLength, layerMask))
        {
            Debug.DrawRay(position, forward * hit.distance, Color.yellow);
            if (hit.collider.isTrigger) return null;
            return hit.distance;
            //Debug.Log("Did Hit: ");
        }

        Debug.DrawRay(position, forward * rayLength, Color.white);
        //Debug.Log("Did not Hit");

        return null;
    }

    private Vector3 CalculateForward()
    {
        Vector3 forward;
        switch (steerTransformForward)
        {
            case Axis.X:
                forward = steerTransform.right;
                break;
            case Axis.Y:
                forward = steerTransform.up;
                break;
            default:
                forward = steerTransform.forward;
                break;
        }

        return backwardIsForward ? -forward : forward;
    }
}