HoHCI-BikeSim/Assets/Scripts/Wheels/CustomWheelCollider.cs

using System;
using System.Linq;
using UnityEngine;

public struct HitInfo
{
    public readonly RaycastHit Hit;
    public readonly float Timestamp;

    public HitInfo(float timestamp, RaycastHit hit)
    {
        this.Timestamp = timestamp;
        this.Hit = hit;
    }
}

public class CustomWheelCollider : MonoBehaviour
{
    public float radius;
    public int numberOfRays;

    public int collisionLayer = 11;
    //[Range(10f,90f)]
    //public float maxDetectableAngle = 45f;

    private Transform t;
    private Vector3[] rayDirections;

    public HitInfo? CurrentHit { private set; get; } = null;
    public int CurrentHitNumber { private set; get; }

    private void Start()
    {
        t = transform;
        rayDirections = CalculateRayDirections();
    }

    private void FixedUpdate()
    {
        var results = new HitInfo?[numberOfRays];
        for (var i = 0; i < numberOfRays; i++)
        {
            results[i] = CastRay(i);
        }

        CurrentHit = results.FirstOrDefault(r => r != null);
        CurrentHitNumber = results.Count(r => r != null);
    }

    public Vector3[] CalculateRayDirections()
    {
        //var startAngle = 90f - maxDetectableAngle;
        //var stopAngle = 90f + maxDetectableAngle;

        var anglePerRot = 360f / numberOfRays;
        var directions = new Vector3[numberOfRays];

        for (var i = 0; i < numberOfRays; i++)
        {
            directions[i] = CalculateRayDirection(i * anglePerRot * Mathf.Deg2Rad);
        }

        return directions;
    }

    private Vector3 CalculateRayDirection(float angle)
    {
        //(1,0,0) rotated by angle
        var x = 1f;
        var y = 0f;
        return new Vector3(0, Mathf.Sin(angle) * x + Mathf.Cos(angle) * y, Mathf.Cos(angle) * x - Mathf.Sin(angle) * y);
    }

    /*private Vector3 RotateOnYZAxis(Vector3 vector, float angle)
    {
        var y = vector.y;
        var z = vector.z;
        return new Vector3(0, Mathf.Sin(angle) * y + Mathf.Cos(angle) * z, Mathf.Cos(angle) * y - Mathf.Sin(angle) * z);
    }*/


    private HitInfo? CastRay(int index)
    {
        var direction = t.TransformDirection(rayDirections[index]);
        var layerMask = 1 << collisionLayer;
        RaycastHit hit;
        //var direction = t.forward;
        //direction = new Vector3(direction.x, direction.z * Mathf.Sin(angle) + direction.y * Mathf.Cos(angle),
        //    direction.z * Mathf.Cos(angle) - direction.y * Mathf.Sin(angle));

        if (Physics.Raycast(t.position, direction, out hit, radius, layerMask))
        {
            Debug.DrawRay(t.position, direction * hit.distance, Color.yellow);
            //Debug.Log("Did Hit: ");
            return new HitInfo(Time.fixedTime, hit);
        }
        else
        {
            Debug.DrawRay(t.position, direction * radius, Color.white);
            return null;
        }
    }
}