Path class

Path.cs

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Linq;

[AddComponentMenu("Paths/Path")]
public class Path : MonoBehaviour, IEnumerable<Path.Segment>
{
    [System.Serializable]
    public class Waypoint : System.ICloneable
    {
        public string name;
        public Vector3 position;
        public float factorBefore;
        public float factorAfter;
        
        public Waypoint()
        {
            name = "";
            position = Vector3.zero;
        }

        public Waypoint(string name, Vector3 position)
        {
            this.name = name;
            this.position = position;
        }
        
        public Waypoint Clone()
        {
            var waypoint = new Waypoint(name, position);
            waypoint.factorBefore = factorBefore;
            waypoint.factorAfter = factorAfter;
            
            return waypoint;
        }

        object System.ICloneable.Clone()
        {
            return Clone();
        }
    }
    
    public interface IEventResponder
    {
        void Respond(Path path, Event pathEvent);
    }
    
    [System.Serializable]
    public class Event : System.ICloneable
    {
        public float t;                         // Event will fire as t passes this value for this spline part
        public string name;
        public float parameter;
        public MonoBehaviour eventResponder;
        public bool notifyPathFollowers;
        
        [System.NonSerialized]
        public bool selected;
        
        public Event(float t, string name, float parameter, MonoBehaviour eventResponder, bool notifyPathFollowers)
        {
            this.t = t;
            this.name = name;
            this.parameter = parameter;
            this.eventResponder = eventResponder;
            this.notifyPathFollowers = notifyPathFollowers;
        }
        
        public Event Clone()
        {
            return new Event(t, name, parameter, eventResponder, notifyPathFollowers);
        }
        
        object System.ICloneable.Clone()
        {
            return Clone();
        }
    }
    
    public List<Waypoint> m_Waypoints = new List<Waypoint>();
    public bool m_Loop;
    
    public List<Event> m_Events = new List<Event>();
    
    public Path m_NextPath;
    public int m_Subdivisions = 8;

#region Editor only
    public Color m_Colour = Color.white;
    public bool m_ShowVertices;
#endregion

    public bool m_ProjectedDownwards;
    
    public IEnumerator<Segment> PathFromStart()
    {
        // TODO: current replacing with Traverse()
        
        var sequence = GetEnumerator();
        sequence.MoveNext();
        return sequence;
    }
    
    public Vector3 ClosestPointOnPath(Vector3 position)
    {
        var point = m_Waypoints[0].position;
        var closestPoint = position;
        var shortestDistance = Mathf.Infinity;
        
        foreach(var segment in Traverse())
        {
            point = segment.nextVertex;
            float distance = (point - position).sqrMagnitude;
            if(distance < shortestDistance)
            {
                closestPoint = point;
                shortestDistance = distance;
            }
        }
        
        return closestPoint;
    }
    
    public IEnumerator<Segment> PathStartingAtPoint(Vector3 position)
    {
        position = ClosestPointOnPath(position);
        var sequence = PathFromStart();
        
        do
        {
            var point = sequence.Current.nextVertex;
            if(point == position)
                break;
        }
        while(sequence.MoveNext());
                
        return sequence;
    }
    
    public Ray startingRay
    {
        get
        {
            var segment = Traverse().First();
            return new Ray(segment.vertex, segment.nextVertex - segment.vertex);
        }
    }
    
    public Path nextPath {get {return m_NextPath;}}
    public bool isLoop {get {return m_Loop;}}

#region Gizmos
    protected void OnDrawGizmos()
    {
        if(m_Waypoints.Count < 2)
            return;
        
        foreach(Segment segment in this)
        {
            Gizmos.color = Color.Lerp(Color.black, (segment.isCurve ? m_Colour + new Color(0.0f, 0.3f, 0.0f) : m_Colour), segment.t);
            Gizmos.DrawLine(segment.vertex, segment.nextVertex);
        }
    }
#endregion
    
#region Enumeration
    public class Segment
    {
        public Vector3 vertex;
        public Vector3 nextVertex;
        public float t;
        public float totalT;
        public bool isCurve;
        
        public Segment(Vector3 vertex, Vector3 nextVertex, float t, float total, bool isCurve)
        {
            this.vertex = vertex;
            this.nextVertex = nextVertex;
            this.t = t;
            this.totalT = total;
            this.isCurve = isCurve;
        }
    }
    
    public IEnumerator<Segment> GetEnumerator()
    {
        if(m_Waypoints == null || m_Waypoints.Count == 0)
            yield break;
        
        Vector3 previousVertex = m_Waypoints[0].position;
        Vector3 previousEndpoint = previousVertex;
        
        if(m_Loop)
            previousEndpoint += (m_Waypoints[1].position - previousVertex) * m_Waypoints[0].factorAfter;
        
        float t = 0;
        int count = m_Waypoints.Count;
        for(int i = 1; i < count + (m_Loop ? 1 : -1); ++i)
        {
            int thisI = i % count;
            int nextI = (i + 1) % count;
            
            Vector3 vertex = m_Waypoints[thisI].position;
            Vector3 vertexBefore = vertex + (previousVertex - vertex) * m_Waypoints[thisI].factorBefore;

            foreach(Segment segment in EvaluateLinearSpline(transform.TransformPoint(previousEndpoint),
                                                            transform.TransformPoint(vertexBefore),
                                                            m_Subdivisions, t))
                yield return segment;
            
            t += 1.0f;
            
            Vector3 vertexAfter = vertex + (m_Waypoints[nextI].position - vertex) * m_Waypoints[thisI].factorAfter;

            foreach(Segment segment in EvaluateQuadraticBezierSpline(transform.TransformPoint(vertexBefore),
                                                                     transform.TransformPoint(vertex),
                                                                     transform.TransformPoint(vertexAfter),
                                                                     m_Subdivisions, t))
                yield return segment;

            t += 1.0f;

            previousVertex = vertex;
            previousEndpoint = vertexAfter;
        }
        
        if(!m_Loop)
        {
            foreach(Segment segment in EvaluateLinearSpline(transform.TransformPoint(previousEndpoint),
                                                            transform.TransformPoint(m_Waypoints[count - 1].position),
                                                            m_Subdivisions, t))
                yield return segment;
        }

        // TODO: should there be a separate kind of enumerable for when you want it to keep looping forever, instead of
        // just going around the loop once?  Maybe that should still be the job of PathFollower.
    }
    
    IEnumerator IEnumerable.GetEnumerator()
    {
        return GetEnumerator();
    }
    
    public IEnumerable<Segment> Traverse()
    {
        foreach(var segment in this)
            yield return segment;
    }
#endregion
    
#region Spline evaluators
    public static IEnumerable<Segment> EvaluateLinearSpline(Vector3 p0, Vector3 p1, int divisions, float startT)
    {
        Vector3 lastSubdivision = p0;
        for(int i = 1; i <= divisions; ++i)
        {
            float t = (float)i / divisions;
            Vector3 subdivision = Vector3.Lerp(p0, p1, t);
            yield return new Segment(lastSubdivision, subdivision, t, startT + t, false);
            lastSubdivision = subdivision;
        }
    }

    public static IEnumerable<Segment> EvaluateQuadraticBezierSpline(Vector3 p0, Vector3 p1, Vector3 p2, int divisions, float startT)
    {
        Vector3 lastSubdivision = p0;
        for(int i = 1; i <= divisions; ++i)
        {
            float t = (float)i / divisions;
            Vector3 subdivision = QuadraticBezier(p0, p1, p2, t);
            yield return new Segment(lastSubdivision, subdivision, t, startT + t, true);
            lastSubdivision = subdivision;
        }
    }

    public static Vector3 QuadraticBezier(Vector3 p0, Vector3 p1, Vector3 p2, float t)
    {
        float oneMinusT = 1.0f - t;
        return oneMinusT * oneMinusT * p0 + 2 * oneMinusT * t * p1 + t * t * p2;
    }
#endregion
}