Avoidance.cs

[ExecuteAlways]
public class Avoidance : MonoBehaviour
{
    public float avoidanceFactor = 1f;
    public float sensorLength = 20;
    public float sensorThickness = 2;
    public float sensorOffset = 5;
    public float horizontalAngle = 23;
    public float upAngle = 23;
    public float downAngle = 23;

    public Sensor Center;
    public Sensor[] RLUD;
    Vector3[] DirRLUD = { Vector3.right, Vector3.left, Vector3.up, Vector3.down };

    int checkFrequency = 30;
    
    [NonSerialized]
    public bool isStuck;
    [NonSerialized] 
    public Vector3 Result;

    int _rand;
    public float GizmoLen = 5f;

    [NonSerialized]
    public bool moveUpIfStuck = true;

    Vector3 currentDampingVelocity;

    void OnValidate() {
        UpdateColliders();
    }

    [ContextMenu("UpdateColliders")]
    void UpdateColliders() {
        var c = RLUD[0].Collider;
        c.height = sensorLength;
        c.radius = sensorThickness;
        var center = c.center;
        center.z = (c.height - c.radius * 2) / 2;
        c.center = center;
        c.transform.localRotation = Quaternion.AxisAngle(Vector3.up, horizontalAngle * Mathf.Deg2Rad);
        c.transform.localPosition = c.transform.localRotation * (Vector3.forward * sensorOffset);


        c = RLUD[1].Collider;
        c.height = sensorLength;
        c.radius = sensorThickness;
        center = c.center;
        center.z = (c.height - c.radius * 2) / 2;
        c.center = center;
        c.transform.localRotation = Quaternion.AxisAngle(Vector3.up, -horizontalAngle * Mathf.Deg2Rad);
        c.transform.localPosition = c.transform.localRotation * (Vector3.forward * sensorOffset);

        c = RLUD[2].Collider;
        c.height = sensorLength;
        c.radius = sensorThickness;
        center = c.center;
        center.z = (c.height - c.radius * 2) / 2;
        c.center = center;
        c.transform.localRotation = Quaternion.AxisAngle(Vector3.right, -upAngle * Mathf.Deg2Rad);
        c.transform.localPosition = c.transform.localRotation * (Vector3.forward * sensorOffset);

        c = RLUD[3].Collider;
        c.height = sensorLength;
        c.radius = sensorThickness;
        center = c.center;
        center.z = (c.height - c.radius * 2) / 2;
        c.center = center;
        c.transform.localRotation = Quaternion.AxisAngle(Vector3.right, downAngle * Mathf.Deg2Rad);
        c.transform.localPosition = c.transform.localRotation * (Vector3.forward * sensorOffset);



        center = Center.Collider.center;
        Center.Collider.height = sensorLength;
        Center.Collider.radius = sensorThickness;

        center.z = (Center.Collider.height - Center.Collider.radius * 2) / 2;
        Center.Collider.center = center;
        Center.Collider.transform.localPosition = Vector3.forward * sensorOffset;
    }

    void OnDrawGizmos()
    {
        Gizmos.color = Color.magenta;
        Gizmos.DrawLine(transform.position, transform.position + Result * GizmoLen);
        if (isStuck)
        {
            Gizmos.color = Color.red;
            Gizmos.DrawWireSphere(transform.position, 5f);
        }
        
    }

    void OnEnable()
    {
        _rand = Random.Range(0, 2);
        StartCoroutine(Loop());
    }

    IEnumerator Loop()
    {
        var wait = new WaitForSecondsRealtime(1f/checkFrequency);
        float lastTime = 0;
        while (true)
        {
            Tick(Time.timeSinceLevelLoad - lastTime);
            lastTime = Time.timeSinceLevelLoad;
            yield return wait;
        }
    }

    void Tick(float deltaTime)
    {
        var local = Vector3.zero;
        isStuck = false;

        if (Center.HasCollision())
        {
            for (var i = 0; i < 4; i++)
            {
                if (RLUD[i].HasCollision()) continue;
                local = DirRLUD[i];
                break;
            }

            if (local == Vector3.zero && moveUpIfStuck)
            {
                isStuck = true;
                var rand = (Center.ContactHash + _rand) % 2 == 0 ? Vector3.right : Vector3.left;
                local = rand + Vector3.up;
            }
            
        }
        else
        {
            var noCollisionCount = 0;
            for (var i = 0; i < 4; i++)
            {
                if (RLUD[i].HasCollision()) continue;
                local += DirRLUD[i];
                noCollisionCount += 1;
            }

            switch (noCollisionCount)
            {
                case 0:
                    local = Center.HasCollision() ? Vector3.right : Vector3.zero;
                    break;
                case 4:
                    local = Vector3.zero;
                    break;
                default:
                {
                    if (noCollisionCount != 1)
                        local /= noCollisionCount;
                    break;
                }
            }
        }

        var world = transform.TransformDirection(local * avoidanceFactor);

        float maxSpeed = 5f;
        float smoothTime = .1f;
        Result = Vector3.SmoothDamp(Result, world, ref currentDampingVelocity, smoothTime, maxSpeed, deltaTime);
    }
}