Second Order Dynamic Movement in Unity

SecondOrderMotion.cs

//This method was derived from the video by t3ssel8r found here: https://www.youtube.com/watch?v=KPoeNZZ6H4s

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

public class SecondOrderMotion : MonoBehaviour
{
    [Range(0.001f, 20)]
    public float F = 2;
    [Range(0, 1)]
    public float Z = 1;
    [Range(-1, 1)]
    public float R = 1;
    public Transform target;

    private float _f, _z, _r = 0;
    private Transform _transform;
    private SecondOrderDynamics[] _dymamics = new SecondOrderDynamics[3];

    // Start is called before the first frame update
    void Start()
    {
        _f = F;
        _z = Z;
        _r = R;
        _transform = GetComponent<Transform>();
        _dymamics[0] = new SecondOrderDynamics(_f, _z, _r, target.position.x);
        _dymamics[1] = new SecondOrderDynamics(_f, _z, _r, target.position.y);
        _dymamics[2] = new SecondOrderDynamics(_f, _z, _r, target.position.z);
    }

    // Update is called once per frame
    void Update()
    {
        if (_f != F || _z != Z || _r != R)
        {
            _f = F;
            _z = Z;
            _r = R;
            _dymamics[0] = new SecondOrderDynamics(_f, _z, _r, target.position.x);
            _dymamics[1] = new SecondOrderDynamics(_f, _z, _r, target.position.y);
            _dymamics[2] = new SecondOrderDynamics(_f, _z, _r, target.position.z);
        }
        float x = _dymamics[0].Update(Time.deltaTime, target.position.x);
        float y = _dymamics[1].Update(Time.deltaTime, target.position.y);
        float z = _dymamics[2].Update(Time.deltaTime, target.position.z);

        _transform.position = new Vector3(x, y, z);
    }

    private void OnDrawGizmos()
    {
        Gizmos.color = Color.cyan;
        Gizmos.DrawWireSphere(target.position, 0.5f);
    }


}

public class SecondOrderDynamics
{
    private float xp; // previous input
    private float y, yd; // state variables
    float _w, _z, _d, k1, k2, k3; // dynamic constraints

    public SecondOrderDynamics(float f, float z, float r, float x0)
    {
        //compute constants
        _w = 2 * Mathf.PI * f;
        _z = z;
        _d = _w * Mathf.Sqrt(Mathf.Abs(z * z - 1));
        k1 = z / (Mathf.PI * f);
        k2 = 1 / ((2*Mathf.PI * f) * (2 * Mathf.PI * f));
        k3 = r * z / (2 * Mathf.PI * f);

        //Initialize Variables
        xp = x0;
        y = x0;
        yd = 0;
    }

    public float Update(float T, float x)
    {
        //Estimate velocity
        float xd = (x - xp) / T;
        xp = x;

        float k1_stable, k2_stable;
        if (_w * T < _z) // clamp k2 to guarantee stability without jitter
        {
            k1_stable = k1;
            k2_stable = Mathf.Max(k2, T*T/2f + T*k1/2f, T*k1);
        }
        else // Use pole matching when the system is very fast
        {
            float t1 = Mathf.Exp(-_z * _w * T);
            float alpha = 2 * t1 * (_z <= 1 ? Mathf.Cos(T * _d) : System.MathF.Cosh(T * _d));
            float beta = t1 * t1;
            float t2 = T / (1 + beta - alpha);
            k1_stable = (1 - beta) * t2;
            k2_stable = T * t2;
        }

        y = y + T * yd; // integrate position by velocity
        yd = yd + T * (x + k3*xd - y - k1*yd)/k2_stable; // integrate velocity by acceleration
        return y;
    }
}

SecondOrderMotionCustomInspector.cs

//The code for the custom unity inspector GUI for the second order motion was
//provided via twitter: https://twitter.com/t3ssel8r/status/1542146007764729857

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEditor;

[CustomEditor(typeof(SecondOrderMotion))]
public class SecondOrderMotionCustomInspector : Editor
{
    private Keyframe[] _keyframes = new Keyframe[60];
    private AnimationCurve _curve;
    private float f, r, z = float.PositiveInfinity;

    // Update is called once per frame
    public override void OnInspectorGUI()
    {
        SecondOrderMotion t = (SecondOrderMotion)target;
        DrawDefaultInspector();

        GUILayout.Space(10);
        Rect rect = EditorGUILayout.GetControlRect(GUILayout.ExpandWidth(true), GUILayout.Height(128));
        float tmax = 2f;
        float dt = Time.fixedDeltaTime;
        float[] y = new float[(int)(tmax / Time.fixedDeltaTime)];
        SecondOrderDynamics dyn = new SecondOrderDynamics(t.F, t.Z, t.R, 0);
        for (int i = 1; i < y.Length; i++)
        {
            y[i] = dyn.Update(dt, 1);
        }
        float ymin = Mathf.Min(0, Mathf.Min(y));
        float ymax = Mathf.Max(1, Mathf.Max(y));

        Vector2 c2p(float t, float x)
        {
            return new Vector2(Mathf.Lerp(rect.xMin, rect.xMax, t / tmax),
            Mathf.Lerp(rect.yMax, rect.yMin, (x - ymin) / (ymax - ymin)));
        }

        Handles.BeginGUI();
        Handles.DrawLine(c2p(0,0), c2p(tmax, 0));
        Handles.DrawLine(c2p(0, ymin), c2p(0, ymax));
        Handles.color = Color.green;
        Handles.DrawLine(c2p(0, 1), c2p(tmax, 1));
        for (int i = 1; i < y.Length; i++)
        {
            Handles.color = Color.cyan;
            Handles.DrawLine(c2p(Time.fixedDeltaTime * (i - 1), y[i - 1]),
            c2p(Time.fixedDeltaTime * i, y[i]));
        }
        Handles.EndGUI();
        GUI.Label(new Rect(c2p(0,0) + Vector2.left * 12, Vector2.one * 12), "0");
        GUI.Label(new Rect(c2p(0,0) + Vector2.left * 12 + Vector2.down * 6, Vector2.one * 12), "1");
        GUI.Label(new Rect(c2p(tmax, 0) - new Vector2(64, 0), new Vector2(64, 32)), tmax.ToString(), new GUIStyle(GUI.skin.label) { alignment = TextAnchor.UpperRight }); 
        EditorGUILayout.GetControlRect(GUILayout.ExpandWidth(true), GUILayout.Height(32));
        
    }
}