VR-Mess/scripts/ArmSpike.cs

using System.Collections.Generic;
using Godot;

// Headless regression for body-physics Stage 2 (PhysicsArms): chase-driven arm segments.
// Run: godot-mono --headless --fixed-fps 90 --path . scenes/arm_spike.tscn
public partial class ArmSpike : Node3D
{
    private enum Phase { Settle, Track, Swing, Push, Pin, Teleport, NanGuard, Done }

    private const float GapLimit = 0.04f;
    private const float SwingGapLimit = 0.12f;
    private const float ChestDisturbLimit = 0.01f;
    private const float PushMinDisplacement = 0.05f;
    private const float PushHandClearance = 0.08f;
    private const float PinSpeedLimit = 0.3f;
    private const float PinBarY = 1.22f;
    private const float PinObstructionMin = 0.01f;
    private const float TeleportGapLimit = 0.04f;
    private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);

    private Node3D _head;
    private PhysicsTorso _torso;
    private PhysicsArms _arms;
    private readonly RigidBody3D[] _hands = new RigidBody3D[2];
    private readonly Transform3D[] _handTarget = new Transform3D[2];
    private readonly Vector3[] _handRest = new Vector3[2];
    private RigidBody3D _box;
    private Vector3 _boxStart;
    private StaticBody3D _slab;

    private Phase _phase = Phase.Settle;
    private float _phaseT;
    private float _maxGap;
    private float _maxChestErr;
    private float _minHandBoxDist;
    private float _maxPinSpeed;
    private float _maxPinObstruction;
    private readonly float[] _pinPrevY = { float.MinValue, float.MinValue };
    private bool _pinTunneled;
    private bool _pinFinite = true;
    private readonly List<string> _names = new();
    private readonly List<bool> _ok = new();

    public override void _Ready()
    {
        StaticBody3D floor = new() { CollisionMask = 0b11101 };
        floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) }, Position = new Vector3(0f, -0.05f, 0f) });
        AddChild(floor);

        _head = new Node3D { Position = new Vector3(0f, 1.65f, 0f) };
        AddChild(_head);
        _torso = new PhysicsTorso { HeadSource = _head };
        AddChild(_torso);
        _torso.Body.GlobalPosition = new Vector3(0f, 1.23f, 0.12f);

        for (int side = 0; side < 2; side++)
        {
            float x = side == 0 ? -0.2f : 0.2f;
            _handRest[side] = new Vector3(x, 1.1f, -0.35f);
            _handTarget[side] = new Transform3D(Basis.Identity, _handRest[side]);
            _hands[side] = new RigidBody3D
            {
                Mass = 0.4f,
                GravityScale = 0f,
                ContinuousCd = true,
                Position = _handRest[side],
                CollisionLayer = 0b1000,
                CollisionMask = 0b1001,
            };
            _hands[side].AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
            AddChild(_hands[side]);
        }

        _arms = new PhysicsArms { HeadSource = _head, Torso = _torso, HandLeft = _hands[0], HandRight = _hands[1] };
        AddChild(_arms);
        GD.Print("ARM: arm spike scene ready");
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        _phaseT += dt;
        for (int side = 0; side < 2; side++)
            PhysicsHands.Drive(_hands[side], _handTarget[side], dt);
        if (!_arms.Calibrated || _arms.UpperArm(0) == null)
            return;

        switch (_phase)
        {
            case Phase.Settle:
                if (_phaseT > 1f)
                    Start(Phase.Track, "track — hands circle, segments must stay connected, chest undisturbed");
                break;

            case Phase.Track:
                for (int side = 0; side < 2; side++)
                {
                    float w = side == 0 ? 2.1f : 2.7f;
                    _handTarget[side].Origin = _handRest[side] + new Vector3(0.15f * Mathf.Sin(w * _phaseT), 0.15f * Mathf.Cos(w * _phaseT), 0.1f * Mathf.Sin(0.5f * w * _phaseT));
                }
                if (_phaseT > 0.5f)
                {
                    _maxGap = Mathf.Max(_maxGap, MaxGap());
                    _maxChestErr = Mathf.Max(_maxChestErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
                }
                if (_phaseT > 3f)
                {
                    Record($"track joint gap max {_maxGap:F3} m (limit {GapLimit})", _maxGap < GapLimit);
                    Record($"chest undisturbed by own arms (err max {_maxChestErr:F4} m, limit {ChestDisturbLimit})", _maxChestErr < ChestDisturbLimit);
                    Start(Phase.Swing, "swing — fast 0.4 m arm swings at 2 Hz, bounded lag");
                }
                break;

            case Phase.Swing:
                _handTarget[1].Origin = _handRest[1] + new Vector3(0.2f * Mathf.Sin(12.6f * _phaseT), 0.1f * Mathf.Sin(6.3f * _phaseT), 0f);
                _maxGap = Mathf.Max(_maxGap, MaxGap());
                if (_phaseT > 2f)
                {
                    _handTarget[1].Origin = _handRest[1];
                    Record($"swing joint gap max {_maxGap:F3} m (limit {SwingGapLimit})", _maxGap < SwingGapLimit);
                    Start(Phase.Push, "push — right forearm swings up through a 1 kg box (hand kept clear)");
                    // Box just above the forearm's middle; the hand rises 0.3 m ~0.2 m away from it.
                    _boxStart = (_arms.ElbowTarget(1) + _arms.WristTarget(1)) * 0.5f + Vector3.Up * 0.11f;
                    _box = new RigidBody3D { Mass = 1f, GravityScale = 0f, Position = _boxStart, CollisionMask = 0b11101 };
                    _box.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.08f } });
                    AddChild(_box);
                    _minHandBoxDist = float.MaxValue;
                }
                break;

            case Phase.Push:
            {
                float k = Mathf.Min(_phaseT / 0.6f, 1f);
                _handTarget[1].Origin = _handRest[1] + Vector3.Up * (0.3f * k);
                _minHandBoxDist = Mathf.Min(_minHandBoxDist, _hands[1].GlobalPosition.DistanceTo(_box.GlobalPosition));
                if (_phaseT > 1.5f)
                {
                    float moved = _box.GlobalPosition.DistanceTo(_boxStart);
                    Record($"arm segment pushes prop (box moved {moved:F3} m, limit > {PushMinDisplacement}; hand-box min dist {_minHandBoxDist:F3} m, limit > {PushHandClearance})",
                        moved > PushMinDisplacement && _minHandBoxDist > PushHandClearance);
                    _box.QueueFree();
                    _handTarget[1].Origin = _handRest[1];
                    // Two short static bars just above the forearms' middles (x clear of the chest).
                    // Hands at (±0.2, 1.17, -0.09) put the forearm centers at ~(±0.22, 1.16, 0.09)
                    // with the elbows swung back; then the hands rise 0.25 m in front of the bars,
                    // so the forearm targets sweep up through them.
                    // The bars appear once the forearms have settled in the pin pose (t = 0.8 s).
                    _maxPinObstruction = 0f;
                    Start(Phase.Pin, "pin — forearms blocked under static bars while the hands rise past them");
                }
                break;
            }

            case Phase.Pin:
            {
                if (_slab == null && _phaseT >= 0.8f)
                {
                    _slab = new StaticBody3D { CollisionMask = 0b11101 };
                    foreach (float x in new[] { -0.22f, 0.22f })
                        _slab.AddChild(new CollisionShape3D { Position = new Vector3(x, 0f, 0f), Shape = new BoxShape3D { Size = new Vector3(0.14f, 0.04f, 0.04f) } });
                    AddChild(_slab);
                    _slab.GlobalPosition = new Vector3(0f, PinBarY, 0.09f);
                    for (int side = 0; side < 2; side++)
                        GD.Print($"ARM: pin start side {side}: forearm center {_arms.Forearm(side).GlobalPosition} (bar y {PinBarY})");
                }
                float rise = Mathf.Clamp((_phaseT - 1f) / 0.5f, 0f, 1f) * 0.25f;
                for (int side = 0; side < 2; side++)
                    _handTarget[side].Origin = new Vector3(side == 0 ? -0.2f : 0.2f, 1.17f + rise, -0.09f);
                if (_slab != null)
                {
                    for (int side = 0; side < 2; side++)
                    {
                        RigidBody3D fore = _arms.Forearm(side);
                        _pinFinite &= BodyMath.IsFinite(fore) && BodyMath.IsFinite(_arms.UpperArm(side));
                        if (_phaseT > 2.5f)
                        {
                            _maxPinSpeed = Mathf.Max(_maxPinSpeed, fore.LinearVelocity.Length());
                            _maxPinSpeed = Mathf.Max(_maxPinSpeed, _arms.UpperArm(side).LinearVelocity.Length());
                        }
                        // Tunneling = the forearm center rises past the bar while still over it in x
                        // (bar half-width 0.07 + forearm half-width 0.03). Sliding off sideways and
                        // then rising is honest physics.
                        Vector3 c = fore.GlobalPosition;
                        bool overBar = Mathf.Abs(Mathf.Abs(c.X) - 0.22f) < 0.1f;
                        if (_pinPrevY[side] > float.MinValue && _pinPrevY[side] < PinBarY && c.Y >= PinBarY && overBar)
                            _pinTunneled = true;
                        if (_pinPrevY[side] < PinBarY && c.Y >= PinBarY)
                            GD.Print($"ARM: side {side} forearm cleared the bar at x {c.X:F3} (over bar: {overBar}) t {_phaseT:F2}");
                        _pinPrevY[side] = c.Y;
                        Vector3 foreTarget = (_arms.ElbowTarget(side) + _arms.WristTarget(side)) * 0.5f;
                        _maxPinObstruction = Mathf.Max(_maxPinObstruction, _arms.Forearm(side).GlobalPosition.DistanceTo(foreTarget));
                    }
                }
                if (_phaseT > 3.5f)
                {
                    Record($"forearm blocked by bar, no tunneling (deviation max {_maxPinObstruction:F3} m, limit > {PinObstructionMin}; tunneled {_pinTunneled})",
                        _maxPinObstruction > PinObstructionMin && !_pinTunneled);
                    Record($"pinned under bar: finite and quiet in the last 1 s (segment |v| max {_maxPinSpeed:F3} m/s, limit {PinSpeedLimit})", _pinFinite && _maxPinSpeed < PinSpeedLimit);
                    _slab.QueueFree();
                    for (int side = 0; side < 2; side++)
                        _handRest[side] += new Vector3(3f, 0f, 0f);
                    _head.GlobalPosition += new Vector3(3f, 0f, 0f);
                    for (int side = 0; side < 2; side++)
                    {
                        _handTarget[side].Origin = _handRest[side];
                        _hands[side].GlobalPosition = _handRest[side];
                        _hands[side].LinearVelocity = Vector3.Zero;
                    }
                    Start(Phase.Teleport, "teleport — head + hands jump 3 m, arms must snap");
                }
                break;
            }

            case Phase.Teleport:
                if (_phaseT > 0.4f)
                {
                    float gap = MaxGap();
                    Record($"teleport re-attach gap {gap:F3} m (limit {TeleportGapLimit})", gap < TeleportGapLimit);
                    _arms.Forearm(0).AngularVelocity = new Vector3(float.NaN, 0f, 0f);
                    Start(Phase.NanGuard, "nan guard — inject NaN into the left forearm, it must reset");
                }
                break;

            case Phase.NanGuard:
                if (_phaseT > 0.3f)
                {
                    bool finite = BodyMath.IsFinite(_arms.Forearm(0));
                    float gap = MaxGap();
                    Record($"non-finite segment reset (finite {finite}, gap {gap:F3} m)", finite && gap < GapLimit);
                    Finish();
                }
                break;
        }
    }

    private float MaxGap()
    {
        float gap = 0f;
        for (int side = 0; side < 2; side++)
        {
            RigidBody3D upper = _arms.UpperArm(side);
            RigidBody3D fore = _arms.Forearm(side);
            Vector3 upperProx = upper.GlobalPosition - upper.GlobalTransform.Basis.Y * (_arms.UpperLen * 0.5f);
            Vector3 upperDist = upper.GlobalPosition + upper.GlobalTransform.Basis.Y * (_arms.UpperLen * 0.5f);
            Vector3 foreProx = fore.GlobalPosition - fore.GlobalTransform.Basis.Y * (_arms.ForeLen * 0.5f);
            gap = Mathf.Max(gap, upperProx.DistanceTo(_arms.Shoulder(side)));
            gap = Mathf.Max(gap, upperDist.DistanceTo(foreProx));
            gap = Mathf.Max(gap, _arms.Wrist(side).DistanceTo(_arms.WristTarget(side)));
        }
        return gap;
    }

    private void Start(Phase phase, string what)
    {
        _phase = phase;
        _phaseT = 0f;
        _maxGap = 0f;
        GD.Print($"ARM: {what}");
    }

    private void Record(string name, bool ok)
    {
        _names.Add(name);
        _ok.Add(ok);
        GD.Print($"ARM: {name} → {(ok ? "PASS" : "FAIL")}");
    }

    private void Finish()
    {
        _phase = Phase.Done;
        bool overall = true;
        foreach (bool ok in _ok)
            overall &= ok;
        GD.Print($"ARM: overall {(overall ? "PASS — physical arms functional on Box3D" : "CHECK — inspect failures above")}");
        GetTree().Quit();
    }
}