VR-Mess/scripts/TrueBodySpike.cs

using System.Collections.Generic;
using System.IO;
using Godot;

// Headless regression for body step 1: T-pose calibration (TPoseDetector), prefs.json persistence,
// and the TrueBodySolver pose. No physics — runs synchronously and quits.
// godot-mono --headless --path . scenes/truebody_spike.tscn
public partial class TrueBodySpike : Node
{
    private const float Dt = 1f / 90f;
    private const float Stature = 1.80f;
    private static float Eye => BodyProfile.EyeToStature * Stature;
    private static float ShoulderY => BodyProfile.ShoulderHeightRatio * Stature;
    private static float HalfWidth => BodyProfile.ShoulderWidthRatio * Stature * 0.5f;
    private static float RealArm => (BodyProfile.UpperArmRatio + BodyProfile.ForearmRatio) * Stature;

    private readonly List<(string Name, bool Ok)> _results = new();
    private readonly RandomNumberGenerator _rng = new() { Seed = 7 };

    public override void _Ready()
    {
        BodyProfile measured = TestDetector();
        TestPersistence(measured);
        TestSolver(measured ?? BodyProfile.Estimate(Eye));
        bool all = true;
        foreach (var (name, ok) in _results)
        {
            GD.Print($"TRUEBODY: {name} → {(ok ? "PASS" : "FAIL")}");
            all &= ok;
        }
        GD.Print($"TRUEBODY: overall {(all ? "PASS — calibration + True Body functional" : "CHECK — inspect failures above")}");
        GetTree().Quit();
    }

    private void Record(string name, bool ok) => _results.Add((name, ok));

    private Vector3 Noise(float amp) => new(_rng.RandfRange(-amp, amp), _rng.RandfRange(-amp, amp), _rng.RandfRange(-amp, amp));

    // ---- calibration ----
    private BodyProfile RunPose(float seconds, System.Func<float, (Vector3 head, Vector3 fwd, Vector3 l, Vector3 r)> pose, TPoseDetector d)
    {
        for (float t = 0f; t < seconds; t += Dt)
        {
            var (head, fwd, l, r) = pose(t);
            BodyProfile p = d.Feed(head, fwd, l, r, Dt);
            if (p != null)
                return p;
        }
        return null;
    }

    private (Vector3, Vector3, Vector3, Vector3) TPose(float t, float leftDrop = 0f, float wobble = 0f, bool armsForward = false, float spanScale = 1f)
    {
        float reach = (HalfWidth + RealArm) * spanScale;
        Vector3 head = new Vector3(0f, Eye, 0f) + Noise(0.0005f);
        Vector3 l = armsForward ? new Vector3(-HalfWidth, ShoulderY, -RealArm) : new Vector3(-reach, ShoulderY - leftDrop, 0f);
        Vector3 r = armsForward ? new Vector3(HalfWidth, ShoulderY, -RealArm) : new Vector3(reach, ShoulderY, 0f);
        Vector3 w = new(0f, 0.05f * Mathf.Sin(t * Mathf.Tau * 2f) * wobble, 0f);
        return (head, Vector3.Forward, l + w + Noise(0.0005f), r - w + Noise(0.0005f));
    }

    private BodyProfile TestDetector()
    {
        var d = new TPoseDetector();
        BodyProfile p = RunPose(1.6f, t => TPose(t), d);
        float expectArm = RealArm - BodyProfile.ArmSafetyMargin;
        bool ok = p != null && Mathf.Abs(p.EyeHeight - Eye) < 0.003f && Mathf.Abs(p.ArmLength - expectArm) < 0.005f
                  && Mathf.Abs(p.ShoulderDrop - (Eye - ShoulderY)) < 0.005f && Mathf.Abs(p.Stature - Stature) < 0.005f;
        Record($"T-pose measured: {(p != null ? p.ToString() : "none")} (expected arm {expectArm:F3}, drop {Eye - ShoulderY:F3})", ok);

        d.Reset();
        Record($"uneven arms rejected ({(RunPose(3f, t => TPose(t, leftDrop: 0.15f), d) == null ? d.Problem : "accepted")})",
            RunPose(0.01f, t => TPose(t, leftDrop: 0.15f), d) == null && d.Problem == "level your arms");
        d.Reset();
        Record($"moving arms rejected ({(RunPose(3f, t => TPose(t, wobble: 1f), d) == null ? d.Problem : "accepted")})",
            RunPose(0.5f, t => TPose(t, wobble: 1f), d) == null);
        d.Reset();
        Record("arms-forward pose rejected", RunPose(3f, t => TPose(t, armsForward: true), d) == null);
        d.Reset();
        Record("implausible span rejected", RunPose(3f, t => TPose(t, spanScale: 1.6f), d) == null);
        return p;
    }

    private void TestPersistence(BodyProfile p)
    {
        if (p == null)
        {
            Record("persistence (no profile to save)", false);
            return;
        }
        string path = Path.Combine(OS.GetUserDataDir(), "truebody_spike_prefs.json");
        bool saved = Prefs.SaveBody(p, path);
        BodyProfile back = Prefs.LoadBody(path);
        bool same = back != null && Mathf.IsEqualApprox(back.ArmLength, p.ArmLength) && Mathf.IsEqualApprox(back.EyeHeight, p.EyeHeight)
                    && Mathf.IsEqualApprox(back.ShoulderHalfWidth, p.ShoulderHalfWidth) && Mathf.IsEqualApprox(back.ShoulderDrop, p.ShoulderDrop);
        Record($"prefs round-trip ({path})", saved && same);
        File.WriteAllText(path, "{ not json");
        Record("corrupt prefs rejected", Prefs.LoadBody(path) == null);
        File.Delete(path);
    }

    // ---- solver ----
    private static Transform3D Ctrl(Vector3 wrist, Basis basis) => new(basis, wrist - basis * new Vector3(0f, 0f, TrueBodySolver.WristOffset));

    private void Settle(TrueBodySolver s, BodyProfile p, Transform3D head, Transform3D l, Transform3D r, float seconds = 1.5f)
    {
        for (float t = 0f; t < seconds; t += Dt)
            s.Solve(p, head, l, r, 0f, Dt);
    }

    private void TestSolver(BodyProfile p)
    {
        var s = new TrueBodySolver();
        Transform3D head = new(Basis.Identity, new Vector3(0f, p.EyeHeight, 0f));
        Basis down = new(Vector3.Right, -Mathf.Pi / 2f);   // controller pointing at the floor (hands hanging)
        float shoulderY = p.EyeHeight - p.ShoulderDrop;

        // Neutral: hands hanging at the sides.
        Vector3 hangL = new(-p.ShoulderHalfWidth, shoulderY - p.ArmLength * 0.95f, 0.06f);
        Vector3 hangR = new(p.ShoulderHalfWidth, shoulderY - p.ArmLength * 0.95f, 0.06f);
        Settle(s, p, head, Ctrl(hangL, down), Ctrl(hangR, down));
        Vector3 expectR = new(p.ShoulderHalfWidth, shoulderY, TrueBodySolver.ShouldersBehindEyes);
        float shoulderErr = Mathf.Max(s.Shoulder[1].DistanceTo(expectR), s.Shoulder[0].DistanceTo(new Vector3(-expectR.X, expectR.Y, expectR.Z)));
        float pelvisExpectY = shoulderY - TrueBodySolver.TrunkRatio * p.Stature;
        Record($"neutral: shoulder joints within {shoulderErr * 100f:F1} cm of anthropometric (limit 2), pelvis y {s.Pelvis.Y:F3} vs {pelvisExpectY:F3}",
            shoulderErr < 0.02f && Mathf.Abs(s.Pelvis.Y - pelvisExpectY) < 0.03f);
        Vector3 neutralShoulderR = s.Shoulder[1];

        // Reach straight forward 10 cm beyond the arm: the shoulder protracts, within its limit.
        Basis fwd = Basis.Identity;
        Vector3 farR = new(p.ShoulderHalfWidth, shoulderY, TrueBodySolver.ShouldersBehindEyes - p.ArmLength - 0.10f);
        Settle(s, p, head, Ctrl(hangL, down), Ctrl(farR, fwd));
        float protraction = neutralShoulderR.Z - s.Shoulder[1].Z;
        float wristErr = s.Wrist[1].DistanceTo(farR);
        Record($"far reach: shoulder protracts {protraction * 100f:F1} cm (4–8.5), wrist short of target by {wristErr * 100f:F1} cm (< 5)",
            protraction > 0.04f && protraction < TrueBodySolver.GirdleForward + 0.005f && wristErr < 0.05f);

        // Arm straight overhead: shrug.
        Vector3 upR = new(p.ShoulderHalfWidth, shoulderY + p.ArmLength * 0.97f, TrueBodySolver.ShouldersBehindEyes);
        Settle(s, p, head, Ctrl(hangL, down), Ctrl(upR, new Basis(Vector3.Right, Mathf.Pi / 2f)));
        float shrug = s.Shoulder[1].Y - neutralShoulderR.Y;
        Record($"overhead: shoulder rises {shrug * 100f:F1} cm (3–7.5)", shrug > 0.03f && shrug < TrueBodySolver.GirdleUp + 0.005f);

        // Look down 60° while standing: chest barely follows, shoulders stay put.
        // A real head pivots at the neck: rotate about the neck pivot, the eyes travel.
        Basis pitch = new(Vector3.Right, -Mathf.DegToRad(60f));
        Vector3 pivot = head.Origin + TrueBodySolver.EyeToNeckPivot;
        Transform3D lookDown = new(pitch, pivot - pitch * TrueBodySolver.EyeToNeckPivot);
        Settle(s, p, lookDown, Ctrl(hangL, down), Ctrl(hangR, down));
        float chestPitch = Mathf.RadToDeg(s.ChestBasis.Y.AngleTo(Vector3.Up));
        Record($"look down 60°: chest pitch {chestPitch:F1}° (< 20), shoulder height change {(s.Shoulder[1].Y - neutralShoulderR.Y) * 100f:F1} cm (|·| < 6)",
            chestPitch < 20f && Mathf.Abs(s.Shoulder[1].Y - neutralShoulderR.Y) < 0.06f);

        // Deep crouch: eyes 0.6 m lower → chest leans, hips kick back behind the upper chest.
        Transform3D crouch = new(Basis.Identity, new Vector3(0f, p.EyeHeight - 0.6f, 0f));
        Settle(s, p, crouch, Ctrl(hangL + Vector3.Down * 0.6f, down), Ctrl(hangR + Vector3.Down * 0.6f, down));
        chestPitch = Mathf.RadToDeg(s.ChestBasis.Y.AngleTo(Vector3.Up));
        float hipBack = s.Pelvis.Z - s.UpperChest.Z;
        Record($"crouch: factor {s.Crouch:F2} (> 0.5), chest pitch {chestPitch:F1}° (> 20), hips {hipBack * 100f:F0} cm behind upper chest (> 10)",
            s.Crouch > 0.5f && chestPitch > 20f && hipBack > 0.10f);

        // Pelvis lag: head snaps 0.3 m sideways → pelvis trails, then settles.
        Settle(s, p, head, Ctrl(hangL, down), Ctrl(hangR, down));
        Transform3D side = new(Basis.Identity, head.Origin + new Vector3(0.3f, 0f, 0f));
        Vector3 before = s.Pelvis;
        s.Solve(p, side, Ctrl(hangL + Vector3.Right * 0.3f, down), Ctrl(hangR + Vector3.Right * 0.3f, down), 0f, Dt);
        float firstStep = s.Pelvis.DistanceTo(before);
        Settle(s, p, side, Ctrl(hangL + Vector3.Right * 0.3f, down), Ctrl(hangR + Vector3.Right * 0.3f, down), 1f);
        float settled = Mathf.Abs(s.Pelvis.X - (s.UpperChest.X));
        Record($"pelvis lag: first tick moves {firstStep * 100f:F1} cm of 30 (< 10), settled offset {settled * 100f:F1} cm (< 1)", firstStep < 0.10f && settled < 0.01f);

        // Random motion: finite, exact segment lengths, no elbow jumps.
        bool finite = true;
        float maxLenErr = 0f, maxElbowStep = 0f;
        Vector3 lastElbow = s.Elbow[1];
        for (int i = 0; i < 900; i++)
        {
            float t = i * Dt;
            Vector3 hp = new(0.2f * Mathf.Sin(t * 0.7f), p.EyeHeight - 0.3f * (0.5f + 0.5f * Mathf.Sin(t * 0.4f)), 0.15f * Mathf.Sin(t * 0.5f));
            Basis hb = new Basis(Vector3.Up, 0.8f * Mathf.Sin(t * 0.3f)) * new Basis(Vector3.Right, 0.4f * Mathf.Sin(t * 0.9f));
            // Hands roam the anatomically reachable space in front of the body (a wrist can't come
            // within ~15 cm of its own shoulder joint; targets that do make the folded arm whip round).
            Vector3 lw = hp + new Vector3(-0.3f + 0.2f * Mathf.Sin(t * 1.3f), -0.35f + 0.35f * Mathf.Sin(t * 1.1f), -0.4f + 0.15f * Mathf.Cos(t * 0.8f));
            Vector3 rw = hp + new Vector3(0.3f + 0.2f * Mathf.Cos(t * 1.7f), -0.35f + 0.35f * Mathf.Cos(t * 0.9f), -0.4f + 0.15f * Mathf.Sin(t * 1.2f));
            Basis lb = new Basis(new Vector3(0.3f, 1f, 0.2f).Normalized(), t * 2f);
            Basis rb = new Basis(new Vector3(-0.2f, 0.4f, 1f).Normalized(), t * 1.5f);
            s.Solve(p, new Transform3D(hb, hp), Ctrl(lw, lb), Ctrl(rw, rb), 0f, Dt);
            for (int a = 0; a < 2; a++)
            {
                finite &= s.Shoulder[a].IsFinite() && s.Elbow[a].IsFinite() && s.Wrist[a].IsFinite() && s.Pelvis.IsFinite();
                maxLenErr = Mathf.Max(maxLenErr, Mathf.Abs(s.Shoulder[a].DistanceTo(s.Elbow[a]) - p.UpperArm));
                maxLenErr = Mathf.Max(maxLenErr, Mathf.Abs(s.Elbow[a].DistanceTo(s.Wrist[a]) - p.Forearm));
            }
            if (i > 0)
            {
                float stepNow = s.Elbow[1].DistanceTo(lastElbow);
                maxElbowStep = Mathf.Max(maxElbowStep, stepNow);
            }
            lastElbow = s.Elbow[1];
        }
        Record($"random motion: finite {finite}, segment length error max {maxLenErr * 1000f:F2} mm (< 1), elbow step max {maxElbowStep * 100f:F1} cm/tick (< 6)",
            finite && maxLenErr < 0.001f && maxElbowStep < 0.06f);
    }
}