VR-Mess/scripts/RagdollDriver.cs

using Godot;

// Smooths one tracked pose for the physics tick. Tracking updates at the display rate (90 Hz) but
// physics runs at 144 Hz: sampled raw, every second or third tick sees the pose frozen and the next
// one jumped, and every velocity feed-forward downstream (muscles, hand guides, ground control)
// turns that into jerks — the step-2.1 "wobbling more and more / sliding around" in the headset.
// Each new sample is timestamped; between samples the pose is extrapolated with the (smoothed)
// sample velocity. Works in the PLAY-SPACE-LOCAL frame, so moving the origin never looks like motion.
public sealed class PoseFilter
{
    public const float MaxExtrapolation = 1.5f / 90f;  // s
    public const float VelocitySmoothing = 0.3f;       // EMA weight of each new sample
    public const float JumpDistance = 0.5f;            // m between samples = recenter, not motion

    private Transform3D _last;
    private double _tLast;
    private bool _has;

    public Vector3 Velocity { get; private set; }         // play-space-local m/s
    public Vector3 AngularVelocity { get; private set; }  // play-space-local rad/s

    public void Reset() => _has = false;

    // sampleTime: when the pose was really measured (the render time of the frame that delivered it),
    // in the same clock as `now`. Without it a sample is timestamped by the first tick that saw it,
    // which at 144 Hz under 90 Hz frames alternates 1 and 2 ticks late: the extrapolated motion
    // then beats at 18 Hz (±30% speed per frame — bible v2, measured). With it the age is exact
    // and motion uniform, at a constant ~2-tick lag. May be negative (a tick before the sample).
    public Transform3D Sample(Transform3D raw, double now, double? sampleTime = null)
    {
        double stamp = sampleTime ?? now;
        if (!_has)
        {
            _has = true;
            _last = raw;
            _tLast = stamp;
            Velocity = AngularVelocity = Vector3.Zero;
            return raw;
        }
        if (raw != _last)
        {
            float interval = (float)(stamp - _tLast);
            if (interval > 1e-4f)
            {
                if (raw.Origin.DistanceTo(_last.Origin) > JumpDistance)
                {
                    Velocity = AngularVelocity = Vector3.Zero;
                }
                else
                {
                    Vector3 v = (raw.Origin - _last.Origin) / interval;
                    Vector3 w = BodyMath.RotationError(raw.Basis.GetRotationQuaternion(), _last.Basis.GetRotationQuaternion()) / interval;
                    Velocity = Velocity.Lerp(v, VelocitySmoothing);
                    AngularVelocity = AngularVelocity.Lerp(w, VelocitySmoothing);
                }
            }
            _last = raw;
            _tLast = stamp;
        }
        float age = Mathf.Clamp((float)(now - _tLast), -MaxExtrapolation, MaxExtrapolation);
        Basis spin = AngularVelocity.LengthSquared() > 1e-10f
            ? new Basis(AngularVelocity.Normalized(), AngularVelocity.Length() * age) : Basis.Identity;
        return new Transform3D(spin * _last.Basis.Orthonormalized(), _last.Origin + Velocity * age);
    }
}

// The per-physics-tick body pipeline, shared by PlayerRig (headset) and RagdollSpike so the
// regression runs exactly what the game runs: filter the tracked poses → play-space correction
// (push-back / ground follow / falls) → True Body solve → (re)build → ragdoll muscles.
// Locomotion (moving the origin by the stick, turning, teleports) happens before Tick.
public sealed class RagdollDriver
{
    public readonly PoseFilter Head = new(), Left = new(), Right = new();
    public TrueBodySolver Solver;
    public PlayerRagdoll Ragdoll;
    public bool RebuildRequested = true;
    // Stick crouch (right stick down), metres: the play-space floor is treated as this far ABOVE where
    // it is. The True Body then reads the head as that much lower (crouch pose), ground control and
    // the view's re-centering settle the play space that far below the ground — so the camera rides
    // down with the crouching body, and nothing feeds back (same play-space-relative rule as 2.5).
    public float StickCrouch;
    private double _now;

    // origin = play space (modified by the correction); raw* = origin-local tracked poses as last
    // reported (repeat between tracking updates); moveVelocity = intended origin velocity (stick),
    // the only origin motion that is real hand motion for the guides' feed-forward.
    // trackingAge: how long ago (s) the current tracking poses were measured, if known (PlayerRig
    // derives it from the render clock); null = timestamp by arrival (the spike's simulated input).
    public void Tick(BodyProfile profile, ref Transform3D origin, Transform3D rawHead, Transform3D rawLeft, Transform3D rawRight,
        Vector3 moveVelocity, float dt, double? trackingAge = null)
    {
        _now += dt;
        double? stamp = trackingAge.HasValue ? _now - trackingAge.Value : null;
        Transform3D head = Head.Sample(rawHead, _now, stamp), left = Left.Sample(rawLeft, _now, stamp), right = Right.Sample(rawRight, _now, stamp);
        if (Ragdoll.Built && !RebuildRequested)
            origin.Origin += Ragdoll.OriginCorrection((origin * head).Origin, origin.Origin.Y + StickCrouch, dt);
        float floorY = origin.Origin.Y + StickCrouch;
        Transform3D h = origin * head, l = origin * left, r = origin * right;
        Solver.Solve(profile, h, l, r, floorY, dt);
        if (RebuildRequested || !Ragdoll.Built)
        {
            Ragdoll.Build(profile, Solver, l, r);
            RebuildRequested = false;
        }
        Vector3 lv = origin.Basis * Left.Velocity + moveVelocity, rv = origin.Basis * Right.Velocity + moveVelocity;
        Ragdoll.Step(Solver, l, r, lv, rv, floorY, dt);
    }
}