Second Order Dynamics for Godot 4 in C#. Used for cool procedural animation stuff.

SecondOrderDynamics.cs

using System;
using Godot;

// based on https://www.youtube.com/watch?v=KPoeNZZ6H4s
// written with generics because repeating code sucks

// rough explanation of the parameters:
// --------------
// f is the frequency of the system, in hz
// things will move faster if this is high basically
// --------------
// z is the damping value
// at 0 it will never stop vibrating
// between 0 and 1 you'll get a cool settling spring motion
// 1 is critical damping, anything above that starts approaching real slow
// --------------
// r determines the response
// if r is less than 1 it will respond slowly
// if r is exactly 1 the system will start to follow the target value immediately
// if r is above 1 the system will overshoot (this is the Cool Zone)'

public abstract class SecondOrderDynamics<T>(float f, float z, float r, T x0)
{
    private T xp = x0; // Previous input
    protected T y = x0; // Current value
    private T yd; // Velocity
    
    // yeah idk about these three, complicated and weird math, watch the video
    private float k1 = z / (Mathf.Pi * f);
    private float k2 = 1 / ((2 * Mathf.Pi * f) * (2 * Mathf.Pi * f));
    private float k3 = r * z / (2 * Mathf.Pi * f);

    protected SecondOrderDynamics(T x0) : this(3f, .5f, 1f, x0)
    {
    }

    protected SecondOrderDynamics(Vector3 parameters, T x0) : this(parameters.X, parameters.Y, parameters.Z, x0)
    {
    }

    public virtual T Update(float dt, T x, bool setVelocity = false, T velocity = default(T))
    {
        // look. the math symbols work with all the other classes, but the compiler doesn't know
        // that i'm just gonna use classes that these are valid for. so. we gotta use dynamic types.
        // because i really don't wanna copy-paste this function to every derived version.
        
        dynamic dynX = x;
        dynamic dynVel = velocity;
        dynamic dyny = y;
        dynamic dynyd = yd;
        
        if (setVelocity == false)
        {
            dynVel = (dynX - xp) / dt;
            xp = x;
        }

        var k2_stable = Mathf.Max(k2, Mathf.Max(dt * dt / 2 + dt * k1 / 2, dt * k1));
        dyny += dt * dynyd;
        dynyd += dt * (x + k3 * dynVel - dyny - k1 * dynyd) / k2_stable;

        y = dyny;
        yd = dynyd;
        
        return y;
    }
}

public class FloatDynamics : SecondOrderDynamics<float>
{
    public FloatDynamics(float x0) : base(x0)
    {
    }

    public FloatDynamics(Vector3 parameters, float x0) : base(parameters, x0)
    {
    }

    public FloatDynamics(float f, float z, float r, float x0) : base(f, z, r, x0)
    {
    }
}

public class Vector2Dynamics : SecondOrderDynamics<Vector2>
{
    public Vector2Dynamics(Vector2 x0) : base(x0)
    {
    }

    public Vector2Dynamics(Vector3 parameters, Vector2 x0) : base(parameters, x0)
    {
    }

    public Vector2Dynamics(float f, float z, float r, Vector2 x0) : base(f, z, r, x0)
    {
    }
}

public class Vector3Dynamics : SecondOrderDynamics<Vector3>
{
    public Vector3Dynamics(Vector3 x0) : base(x0)
    {
    }

    public Vector3Dynamics(Vector3 parameters, Vector3 x0) : base(parameters, x0)
    {
    }

    public Vector3Dynamics(float f, float z, float r, Vector3 x0) : base(f, z, r, x0)
    {
    }
}

public class Vector4Dynamics : SecondOrderDynamics<Vector4>
{
    public Vector4Dynamics(Vector4 x0) : base(x0)
    {
    }

    public Vector4Dynamics(Vector3 parameters, Vector4 x0) : base(parameters, x0)
    {
    }

    public Vector4Dynamics(float f, float z, float r, Vector4 x0) : base(f, z, r, x0)
    {
    }
}

public class QuaternionDynamics : SecondOrderDynamics<Quaternion>
{
    public QuaternionDynamics(Quaternion x0) : base(x0)
    {
    }

    public QuaternionDynamics(Vector3 parameters, Quaternion x0) : base(parameters, x0)
    {
    }

    public QuaternionDynamics(float f, float z, float r, Quaternion x0) : base(f, z, r, x0)
    {
    }

    public override Quaternion Update(float dt, Quaternion x, bool setVelocity = false, Quaternion velocity = default(Quaternion))
    {
        NormalizeSign(y, ref x);
        return base.Update(dt, x, setVelocity, velocity).Normalized();
    }
    
    private static void NormalizeSign(Quaternion current, ref Quaternion target)
    {
        // if our dot product is positive, we don't need to invert signs.
        if (current.Dot(target) >= 0)
        {
            return;
        }

        // invert the signs on the components
        target.X *= -1;
        target.Y *= -1;
        target.Z *= -1;
        target.W *= -1;
    }
}