Avoidance.cs

using System;
using System.Collections;
using UnityEngine;
using Random = UnityEngine.Random;

using Unity.Collections;
using Unity.Jobs;

[ExecuteAlways]
public class Avoidance : MonoBehaviour
{
    public float avoidanceFactor = 1f;
    public float maxSensorLength = 30;
    public float sensorThickness = 2;

    public Transform[] sensors;

    Vector3[] DirRLUD = { Vector3.right, Vector3.left, Vector3.up, Vector3.down };

    [NonSerialized]
    public float currentForwardSpeed = 20;
    [NonSerialized]
    public Vector3 currentVelocity;

    [NonSerialized]
    public bool isStuck;
    [NonSerialized]
    public Vector3 Result;
    [NonSerialized]
    public bool moveUpIfStuck = true;

    Vector3 currentDampingVelocity;

    public LayerMask mask;

    public bool showDebug = false;

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


    void Update() {
        var local = Vector3.zero;
        isStuck = false;

        var results = new NativeArray<RaycastHit>(sensors.Length, Allocator.TempJob);
        var commands = new NativeArray<SpherecastCommand>(sensors.Length, Allocator.TempJob);

        if (currentForwardSpeed > 6) {
            sensors[4].forward = currentVelocity.normalized;
        } else {
            sensors[4].rotation = Quaternion.identity;
        }

        for (int i = 0; i < sensors.Length; i++) {
            float sensorLength = Mathf.Clamp(currentForwardSpeed * 2, 8, i != 4 ? maxSensorLength : maxSensorLength * 1.5f);
            float l = i == 3 ? 10 : sensorLength;
            commands[i] = new SpherecastCommand(sensors[i].position, sensorThickness, sensors[i].forward, l, mask);
        }

        SpherecastCommand.ScheduleBatch(commands, results, 1, default(JobHandle)).Complete();

        if (showDebug) {
            for (int i = 0; i < 5; i++) {
                Debug.DrawLine(sensors[i].position, sensors[i].position + sensors[i].forward * commands[i].distance, results[i].collider == null ? Color.green : Color.red);
            }
        }

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

            isStuck = local == Vector3.zero;

            if (isStuck && moveUpIfStuck) {
                var rand = (Random.Range(0, 100)) % 2 == 0 ? Vector3.right : Vector3.left;
                local = rand + Vector3.up;
            }
        } else {
            var noCollisionCount = 0;
            for (var i = 0; i < 4; i++) {
                if (results[i].collider) continue;
                local += DirRLUD[i];
                noCollisionCount += 1;
            }

            switch (noCollisionCount) {
                case 0:
                    local = results[4].collider != null ? 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, Time.deltaTime / 2);

        results.Dispose();
        commands.Dispose();
    }
}