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;
    }
}

honestly I don't know if this is the place to ask, but I've been struggling to implement SOD for quaternions for like 3 days now and I really wonder how this works. can you add and subtract 2 quats from eachother? weird behavior happens when I didn't invert it, weird jitters happen if I do normalize the sign. If you could please explain I would really appreciate it

If you want to combine Quaternions, you don't add or subtract one from the other - you multiply them together. The order matters, too.

this is why I am confused, because in this code, other than normalizing the sign beforehand, doesn't it just use the exact same formula for quats as with vecs?

this is why I am confused, because in this code, other than normalizing the sign beforehand, doesn't it just use the exact same formula for quats as with vecs?

Yes? That's the point of using generics.

// Up direction:
        
        var footDir0 = _feet[0].FootNode.GlobalPosition.DirectionTo(GlobalPosition);
        var footDir1 = _feet[1].FootNode.GlobalPosition.DirectionTo(GlobalPosition);
        var footUp = footDir0.Lerp(footDir1, .5f).Normalized();
        var footQuat = new Quaternion(Vector3.Up, footUp);

        if (stableFoot == null)
        {
            footQuat = footQuat.Slerp(Godot.Quaternion.Identity, Mathf.Pow(1f - _character.SpeedFactor,2f));
        }
        
        _filteredVelocity = _filteredVelocity.ExpDecay(_character.Velocity.Horizontal(), dt, .2f);
        var acc = _character.Velocity.Horizontal() - _filteredVelocity;

        var accStr = acc.Length();
        var accDir = acc.Normalized();

        var accRight = accDir.Cross(Vector3.Up);

        if (accRight.IsNormalized() == false) accRight = Basis.X;
        var accLean = new Quaternion(accRight, -Mathf.DegToRad(accStr*3));
        
        var lean = new Quaternion(-Basis.X, crouching ? Mathf.DegToRad(60) : _character.SpeedFactor * BodySpeedLean);
        var targetRot = footQuat * accLean * lean * Quaternion;
        
        BodyNode.Quaternion = _bodyRotationDynamics.Update(dt, targetRot);

Some of the code from my character animation. Might help you understand how to use the quaternions.