VR-Mess/scripts/Calibration.cs

using Godot;

// Detects a held T-pose from head + wrist positions in XR-origin space (floor at y = 0) and
// measures it. Pure logic so headless spikes can feed it synthetic poses.
public sealed class TPoseDetector
{
    public const float HoldTime = 1.0f;
    public const float StillRadius = 0.03f;   // every point must stay within this of where the hold began (jitter-proof)
    private const float LevelTolerance = 0.08f;
    private const float SymmetryTolerance = 0.10f;
    private const float MaxLineVsForward = 0.35f;
    private const float MinSpan = 1.0f;
    private const float MaxSpan = 2.4f;
    private const float MinDropBelowEye = 0.05f;
    private const float MaxDropBelowEye = 0.5f;

    private Vector3 _anchorHead, _anchorLeft, _anchorRight;
    private float _held;
    private float _sumEye, _sumSpan, _sumWristY;
    private int _samples;

    public string Problem { get; private set; } = "";
    public float Progress => Mathf.Clamp(_held / HoldTime, 0f, 1f);

    public void Reset()
    {
        _held = 0f;
        _sumEye = _sumSpan = _sumWristY = 0f;
        _samples = 0;
        Problem = "";
    }

    // Returns a profile once the pose has been valid and still for HoldTime.
    public BodyProfile Feed(Vector3 head, Vector3 headForward, Vector3 wristL, Vector3 wristR, float dt)
    {
        Problem = Validate(head, headForward, wristL, wristR);
        bool still = true;
        if (Problem.Length == 0 && _samples > 0)
            still = head.DistanceTo(_anchorHead) < StillRadius && wristL.DistanceTo(_anchorLeft) < StillRadius
                    && wristR.DistanceTo(_anchorRight) < StillRadius;
        if (Problem.Length == 0 && !still)
            Problem = "hold still";
        if (Problem.Length > 0)
        {
            _held = 0f;
            _sumEye = _sumSpan = _sumWristY = 0f;
            _samples = 0;
            return null;
        }

        if (_samples == 0)
        {
            _anchorHead = head;
            _anchorLeft = wristL;
            _anchorRight = wristR;
        }
        _held += dt;
        _sumEye += head.Y;
        _sumSpan += wristL.DistanceTo(wristR);
        _sumWristY += (wristL.Y + wristR.Y) * 0.5f;
        _samples++;
        if (_held < HoldTime)
            return null;

        BodyProfile profile = BodyProfile.FromTPose(_sumEye / _samples, _sumSpan / _samples, _sumWristY / _samples);
        if (!profile.IsPlausible())
        {
            Problem = "measurement implausible — try again";
            Reset();
            return null;
        }
        return profile;
    }

    private static string Validate(Vector3 head, Vector3 headForward, Vector3 wristL, Vector3 wristR)
    {
        if (Mathf.Abs(wristL.Y - wristR.Y) > LevelTolerance)
            return "level your arms";
        float drop = head.Y - (wristL.Y + wristR.Y) * 0.5f;
        if (drop < MinDropBelowEye || drop > MaxDropBelowEye)
            return "arms out at shoulder height";
        float span = wristL.DistanceTo(wristR);
        if (span < MinSpan || span > MaxSpan)
            return "stretch both arms fully out";
        Vector2 headXZ = new(head.X, head.Z);
        float dl = headXZ.DistanceTo(new Vector2(wristL.X, wristL.Z));
        float dr = headXZ.DistanceTo(new Vector2(wristR.X, wristR.Z));
        if (Mathf.Abs(dl - dr) > SymmetryTolerance)
            return "arms out evenly to both sides";
        Vector3 line = wristR - wristL;
        line.Y = 0f;
        Vector3 fwd = new(headForward.X, 0f, headForward.Z);
        if (line.LengthSquared() > 1e-6f && fwd.LengthSquared() > 1e-6f
            && Mathf.Abs(line.Normalized().Dot(fwd.Normalized())) > MaxLineVsForward)
            return "face forward, arms to the sides";
        return "";
    }
}

// One-time body calibration gate. On launch: load prefs.json; if there is no valid profile the
// body systems wait (they check BodyProfile.Current) and a prompt asks for a T-pose. Recalibrate()
// (menu button; group "calibration") repeats it at any time.
public partial class Calibration : Node
{
    public const float WristOffset = 0.06f;  // wrist sits behind the aim-pose origin along its +Z

    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;
    private Label3D _prompt;
    private readonly TPoseDetector _detector = new();
    private bool _active;
    private float _doneTimer;

    public override void _Ready()
    {
        AddToGroup("calibration");
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _prompt = new Label3D
        {
            FontSize = 48,
            PixelSize = 0.0012f,
            OutlineSize = 10,
            NoDepthTest = true,
            Visible = false,
            Position = new Vector3(0f, -0.05f, -1.1f),
        };
        _camera.AddChild(_prompt);

        BodyProfile saved = Prefs.LoadBody();
        if (saved != null)
        {
            BodyProfile.Set(saved);
            GD.Print($"Calibration: loaded {Prefs.FilePath}: {saved}");
        }
        else
        {
            GD.Print($"Calibration: no valid profile at {Prefs.FilePath} — T-pose calibration required");
            Begin();
        }
    }

    public void Recalibrate()
    {
        GD.Print("Calibration: recalibration requested");
        Begin();
    }

    private void Begin()
    {
        _active = true;
        _doneTimer = 0f;
        _detector.Reset();
        _prompt.Visible = true;
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        if (_doneTimer > 0f)
        {
            _doneTimer -= dt;
            if (_doneTimer <= 0f)
                _prompt.Visible = false;
        }
        if (!_active)
            return;

        // Origin-local: the floor is y = 0 and world locomotion doesn't matter.
        Vector3 head = _camera.Position;
        Vector3 forward = _camera.Transform.Basis * Vector3.Forward;
        Vector3 wristL = _left.Transform * new Vector3(0f, 0f, WristOffset);
        Vector3 wristR = _right.Transform * new Vector3(0f, 0f, WristOffset);
        BodyProfile result = _detector.Feed(head, forward, wristL, wristR, dt);

        if (result == null)
        {
            string hint = _detector.Problem.Length > 0 ? _detector.Problem : $"hold… {Mathf.RoundToInt(_detector.Progress * 100f)}%";
            _prompt.Text = $"Body calibration\nStand straight, arms out to the sides (T-pose)\n{hint}";
            return;
        }

        _active = false;
        BodyProfile.Set(result);
        bool saved = Prefs.SaveBody(result);
        _prompt.Text = $"Calibrated ✓\narm {result.ArmLength * 100f:F1} cm · height {result.Stature * 100f:F0} cm";
        _doneTimer = 2.5f;
        GD.Print($"Calibration: T-pose measured: {result}; saved {(saved ? Prefs.FilePath : "FAILED")}");
    }
}