Driftology/Assets/Waypoint System/WaypointCircuit.cs

//Shady

using UnityEngine;
using System.Collections.Generic;


public class WaypointCircuit : MonoBehaviour
{

    //===================================================
    // FIELDS
    //===================================================
    [SerializeField] bool _smoothRoute = true;
    [SerializeField] bool _closedLoop = true;
    [Range(100f, 1000f)][SerializeField] float _editorVisualisationSubsteps = 100f;
    [SerializeField] List<Transform> _waypoints = new List<Transform>();
    public bool accepted = false;

    //===================================================
    // PRIVATE FIELDS
    //===================================================
    private int numPoints;
    private Vector3[] points;
    private float[] distances;

    //this being here will save GC allocs
    private int p0n;
    private int p1n;
    private int p2n;
    private int p3n;

    private float i;
    private Vector3 P0;
    private Vector3 P1;
    private Vector3 P2;
    private Vector3 P3;

    //===================================================
    // PROPERTIES
    //===================================================
    public float Length { get; private set; }

    //===================================================
    // METHODS
    //===================================================
    private void Awake()
    {
        if (_waypoints.Count > 1)
            CachePositionsAndDistances();
        numPoints = _waypoints.Count;
    } //Awake() end

    public RoutePoint GetRoutePoint(float dist)
    {
        // position and direction
        Vector3 p1 = GetRoutePosition(dist);
        Vector3 p2 = GetRoutePosition(dist + 0.1f);
        Vector3 delta = p2 - p1;
        return new RoutePoint(p1, delta.normalized);
    }

    public Vector3 GetRoutePosition(float dist)
    {
        int point = 0;
        if (Length == 0)
        {
            Length = distances[distances.Length - 1];
        }

        dist = Mathf.Repeat(dist, Length);
        while (distances[point] < dist)
        {
            ++point;
        }

        // get nearest two points, ensuring points wrap-around start & end of circuit
        p1n = ((point - 1) + numPoints) % numPoints;
        p2n = point;

        // found point numbers, now find interpolation value between the two middle points
        i = Mathf.InverseLerp(distances[p1n], distances[p2n], dist);
        if (_smoothRoute)
        {
            // smooth catmull-rom calculation between the two relevant points

            // get indices for the surrounding 2 points, because
            // four points are required by the catmull-rom function
            // p0n = ((point - 2) + numPoints) % numPoints;
            p0n = ((point - 1) + numPoints) % numPoints;
            p3n = (point + 1) % numPoints;

            // 2nd point may have been the 'last' point - a dupe of the first,
            // (to give a value of max track distance instead of zero)
            // but now it must be wrapped back to zero if that was the case.
            p2n = p2n % numPoints;
            P0 = points[p0n];
            P1 = points[p1n];
            P2 = points[p2n];
            P3 = points[p3n];
            return CatmullRom(P0, P1, P2, P3, i);
        }
        else
        {
            // simple linear lerp between the two points:
            p1n = ((point - 1) + numPoints) % numPoints;
            p2n = point;
            return Vector3.Lerp(points[p1n], points[p2n], i);
        }
    }

    private Vector3 CatmullRom(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float i)
    {
        // comments are no use here... it's the catmull-rom equation.
        // Un-magic this, lord vector!
        return 0.5f * ((2 * p1) + (-p0 + p2) * i + (2 * p0 - 5 * p1 + 4 * p2 - p3) * i * i + (-p0 + 3 * p1 - 3 * p2 + p3) * i * i * i);
    }

    private void CachePositionsAndDistances()
    {
        // transfer the position of each point and distances between points to arrays for
        // speed of lookup at runtime
        points = new Vector3[_waypoints.Count + 1];
        distances = new float[_waypoints.Count + 1];
        float accumulateDistance = 0;
        for (int i = 0; i < points.Length; ++i)
        {
            var t1 = _waypoints[(i) % _waypoints.Count];
            var t2 = _waypoints[(i + 1) % _waypoints.Count];
            if (t1 != null && t2 != null)
            {
                Vector3 p1 = t1.position;
                Vector3 p2 = t2.position;
                points[i] = _waypoints[i % _waypoints.Count].position;
                distances[i] = accumulateDistance;
                accumulateDistance += (p1 - p2).magnitude;
            } //if end
        }     //loop end
    }         //CachePositionsAndDistances() end

    private void OnDrawGizmos() => DrawGizmos(false);

    private void OnDrawGizmosSelected() => DrawGizmos(true);

    private void DrawGizmos(bool selected)
    {
        // waypointList.circuit = this;
        if (_waypoints.Count > 1)
        {
            numPoints = _waypoints.Count;
            CachePositionsAndDistances();
            Length = distances[distances.Length - 1];
            Gizmos.color = selected ? Color.yellow : Color.yellow;
            Vector3 prev = _waypoints[0].position;
            if (_smoothRoute)
            {
                for (float dist = 0; dist < Length; dist += Length / _editorVisualisationSubsteps)
                {
                    Vector3 next = GetRoutePosition(dist + 1);
                    if (Vector3.Distance(next, _waypoints.Last().position) < Length / _editorVisualisationSubsteps)
                    {
                        // Gizmos.DrawLine(next, _waypoints.Last().position);
                        break;
                    } //if end

                    Gizmos.DrawLine(prev, next);
                    prev = next;
                } //loop end

                if (_closedLoop)
                    Gizmos.DrawLine(_waypoints.Last().position, _waypoints[0].position);
            } //if end
            else
            {
                for (int n = 0; n < _waypoints.Count; ++n)
                {
                    if (n == _waypoints.Count - 1)
                        break;
                    Vector3 next = _waypoints[(n + 1) % _waypoints.Count].position;
                    Gizmos.DrawLine(prev, next);
                    prev = next;
                } //loop end

                if (_closedLoop)
                    Gizmos.DrawLine(_waypoints.Last().position, _waypoints[0].position);
            } //else end
        }     //if end

        foreach (Transform waypoint in _waypoints)
        {
            Gizmos.color = Color.magenta;
            Gizmos.DrawSphere(waypoint.position, 1f);
        } //loop end
    }     //DrawGizmos() end

    private void AutoRename()
    {
        if (_waypoints.Count > 1)
        {
            int n = 0;
            foreach (Transform child in _waypoints)
                child.name = "Waypoint " + (n++).ToString("000");
        } //if end
    }     //AutoRename() end

    private void AssignUsingChild()
    {
        _waypoints = new List<Transform>();
        for (int i = 0; i < transform.childCount; i++)
            _waypoints.Add(transform.GetChild(i));
    } //AssignUsingChild() end


    private void PlaceToGround()
    {
        foreach (Transform trans in _waypoints)
        {
            //define ray to cast downwards waypoint position
            Ray ray = new Ray(trans.position + new Vector3(0, 10f, 0), -Vector3.up);
#if UNITY_EDITOR
            UnityEditor.Undo.RecordObject(trans, "Place To Ground");
#endif
            RaycastHit hit;
            //cast ray against ground, if it hit:
            if (Physics.Raycast(ray, out hit, Mathf.Infinity))
                trans.position = hit.point;
        } //loop end

        LookRotation();
    } //PlaceToGround() end

    public void PlaceWaypoint(Vector3 placePos)
    {
        //instantiate waypoint gameobject
        GameObject wayp = new GameObject("Waypoint " + _waypoints.Count.ToString("000"));
        wayp.transform.position = placePos;
        wayp.transform.rotation = Quaternion.identity;

        //Setting position on Ground
        Ray ray = new Ray(wayp.transform.position + new Vector3(0, 10f, 0), -Vector3.up);
        //cast ray against ground, if it hit:
        if (Physics.Raycast(ray, out RaycastHit hit, 100))
            wayp.transform.position = hit.point;
        wayp.transform.SetParent(transform);
        _waypoints.Add(wayp.transform);
        LookRotation();
    } //PlaceWaypoint() end


    //Editor Only
    private void LookRotation()
    {
        if (_waypoints.Count > 1)
        {
            for (int i = 0; i < _waypoints.Count - 1; i++)
            {
                _waypoints[i].LookAt(_waypoints[i + 1].transform);
                _waypoints[i + 1].transform.rotation = _waypoints[i].transform.rotation;
            }
        }
    }

    private void RemoveWaypoint(Transform waypoint)
    {
        _waypoints.Remove(waypoint);
        DestroyImmediate(waypoint.gameObject);
    } //RemoveWayPoint() end

} //class end