CatmullRomCurve.cs

using UnityEngine;
 
namespace ARLocation
{
    public class CatmullRomCurve : Curve
    {
        // The control points
        private Vector3 p0;
        private Vector3 p1;
        private Vector3 p2;
        private Vector3 p3;
 
        // The alpha/tension factor
        private float alpha;
 
        // The knots
        private float t0;
        private float t1;
        private float t2;
        private float t3;
 
        // Getters for the knots
        public float T0 { get { return t0; } }
        public float T1 { get { return t1; } }
        public float T2 { get { return t2; } }
        public float T3 { get { return t3; } }
 
        private float length;
 
        private float[] sampleParameters;
 
        private float[] sampleLengths;
 
        private bool isSampleDirty;
 
        private int lastSampleSize = 100;
 
        public float Alpha
        {
            get
            {
                return alpha;
            }
            set
            {
                alpha = value;
                CalculateKnots();
 
                isSampleDirty = true;
            }
        }
 
        public Vector3 P0
        {
            get
            {
                return p0;
            }
            set
            {
                p0 = value;
                CalculateKnots();
 
                isSampleDirty = true;
            }
        }
 
        public Vector3 P1
        {
            get
            {
                return p1;
            }
            set
            {
                p1 = value;
                CalculateKnots();
 
                isSampleDirty = true;
            }
        }
 
        public Vector3 P2
        {
            get
            {
                return p2;
            }
            set
            {
                p2 = value;
                CalculateKnots();
 
                isSampleDirty = true;
            }
        }
 
        public Vector3 P3
        {
            get
            {
                return p3;
            }
            set
            {
                p3 = value;
                CalculateKnots();
 
                isSampleDirty = true;
            }
        }
 
        public CatmullRomCurve(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float alpha)
        {
            this.p0 = p0;
            this.p1 = p1;
            this.p2 = p2;
            this.p3 = p3;
 
            this.alpha = alpha;
 
            isSampleDirty = true;
 
            CalculateKnots();
        }
 
        private void CalculateKnots()
        {
            var x = alpha * 0.5f;
            t0 = 0.0f;
            t1 = t0 + Mathf.Pow((p1 - p0).sqrMagnitude, x);
            t2 = t1 + Mathf.Pow((p2 - p1).sqrMagnitude, x);
            t3 = t2 + Mathf.Pow((p3 - p2).sqrMagnitude, x);
        }
 
        public override Vector3 GetPoint(float u)
        {
            u = Mathf.Clamp(u, 0, 1);
            float t = t1 * (1 - u) + t2 * u;
 
            Vector3 a1 = (t1 - t) / (t1 - t0) * p0 + ((t - t0) / (t1 - t0)) * p1;
            Vector3 a2 = (t2 - t) / (t2 - t1) * p1 + ((t - t1) / (t2 - t1)) * p2;
            Vector3 a3 = (t3 - t) / (t3 - t2) * p2 + ((t - t2) / (t3 - t2)) * p3;
 
            Vector3 b1 = (t2 - t) / (t2 - t0) * a1 + ((t - t0) / (t2 - t0)) * a2;
            Vector3 b2 = (t3 - t) / (t3 - t1) * a2 + ((t - t1) / (t3 - t1)) * a3;
 
            Vector3 c = (t2 - t) / (t2 - t1) * b1 + ((t - t1) / (t2 - t1)) * b2;
 
            return c;
        }
 
        public override CurvePointData GetPointAndTangent(float u)
        {
            u = Mathf.Clamp(u, 0, 1);
            float t = t1 * (1 - u) + t2 * u;
 
            Vector3 a1 = (t1 - t) / (t1 - t0) * p0 + ((t - t0) / (t1 - t0)) * p1;
            Vector3 a2 = (t2 - t) / (t2 - t1) * p1 + ((t - t1) / (t2 - t1)) * p2;
            Vector3 a3 = (t3 - t) / (t3 - t2) * p2 + ((t - t2) / (t3 - t2)) * p3;
 
            Vector3 b1 = (t2 - t) / (t2 - t0) * a1 + ((t - t0) / (t2 - t0)) * a2;
            Vector3 b2 = (t3 - t) / (t3 - t1) * a2 + ((t - t1) / (t3 - t1)) * a3;
 
            Vector3 c = (t2 - t) / (t2 - t1) * b1 + ((t - t1) / (t2 - t1)) * b2;
 
            Vector3 da1 = (1.0f / (t1 - t0)) * (p1 - p0);
            Vector3 da2 = (1.0f / (t2 - t1)) * (p2 - p1);
            Vector3 da3 = (1.0f / (t3 - t2)) * (p3 - p2);
 
            Vector3 db1 = (t2 - t) / (t2 - t0) * da1 + ((t - t0) / (t2 - t0)) * da2 +
                (1.0f / (t2 - t0)) * (a2 - a1);
            Vector3 db2 = (t3 - t) / (t3 - t1) * da2 + ((t - t1) / (t3 - t1)) * da3 +
                (1.0f / (t3 - t1)) * (a3 - a2);
 
            Vector3 dc = (t2 - t) / (t2 - t1) * db1 + ((t - t1) / (t2 - t1)) * db2 +
                (1.0f / (t2 - t1)) * (b2 - b1);
 
 
            return new CurvePointData { point = c, tangent = dc };
        }
 
        public override Vector3[] Sample(int n)
        {
            lastSampleSize = n;
 
            if (n == 0)
            {
                return new[] { GetPoint(0), GetPoint(1) };
            }
 
            var sample = new Vector3[n + 2];
            var delta = 1.0f / (n + 1.0f);
            length = 0.0f;
 
            sampleParameters = new float[n + 2];
            sampleLengths = new float[n + 2];
 
            for (var i = 0; i < (n + 2); i++)
            {
                sample[i] = GetPoint(i * delta);
                sampleParameters[i] = i * delta;
 
                if (i > 0)
                {
                    length += (sample[i] - sample[i - 1]).magnitude;
                }
 
                sampleLengths[i] = length;
            }
 
            isSampleDirty = false;
 
            return (Vector3[])sample.Clone();
        }
 
        public override float EstimateLength(int n = 100)
        {
            if (isSampleDirty || lastSampleSize != n)
            {
                Sample(n);
            }
 
            return length;
        }
 
        public override float GetParameterForLength(float s)
        {
            if (isSampleDirty)
            {
                Sample(lastSampleSize);
            }
 
            for (var i = 0; i < (sampleParameters.Length - 1); i++)
            {
                if (s >= sampleLengths[i] && s <= sampleLengths[i + 1])
                {
                    var a = (s - sampleLengths[i]) / (sampleLengths[i + 1] - sampleLengths[i]);
                    return (1 - a) * sampleParameters[i] + a * sampleParameters[i + 1];
                }
            }
 
            return -1.0f;
        }
 
        public override Vector3 GetPointAtLength(float s)
        {
            return GetPoint(GetParameterForLength(s));
        }
 
        public override CurvePointData GetPointAndTangentAtLength(float s)
        {
            return GetPointAndTangent(GetParameterForLength(s));
        }
    }
}