Body Stage 1 collapse root-caused + fixed: NaN in torso servo math (GetAngle = 2*acos(W) at W=1.0000001), not solver stacking

- BodyMath.RotationError: atan2-based shortest-arc error (no NaN, no GetAxis gain collapse
  inside 0.16 deg); used by PhysicsTorso orientation servo and PhysicsHands rotation lock
- BodyMath.ResetIfNonFinite escape hatch on torso + hands
- PhysicsHands.Drive extracted (static) so spikes run the real hand chase
- CollapseSpike: deterministic repro (slow yaw drift across a parked chest, 7/40 NaN on old
  code) + solver-stacking scenarios (seated chest in table, two-hand-welded 50 kg anvil,
  real PropShop) — 49/49 clean after fix, x3; all other spikes PASS x3
- Failure brief RESOLUTION section; HANDOFF/AGENTS updated

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

scripts/BodyMath.cs

@@ -0,0 +1,38 @@
+using Godot;
+
+// Shared math for the chase-driven player bodies (hands, torso, arm segments).
+public static class BodyMath
+{
+    // Shortest-arc rotation taking `current` to `target`, as axis * angle (radians), world space.
+    // Never use Quaternion.GetAngle()/GetAxis() for servo errors: GetAngle() is 2*acos(W) and
+    // returns NaN when float rounding puts W at 1.0000001 (the Stage-1 chest collapse), and
+    // GetAxis() returns the raw un-normalized xyz once W > 1 - 1e-6 (servo gain collapses
+    // ~1000x inside 0.16 deg). atan2 is exact near zero and can't leave its domain.
+    public static Vector3 RotationError(Quaternion target, Quaternion current)
+    {
+        Quaternion q = target * current.Inverse();
+        if (q.W < 0f)
+            q = new Quaternion(-q.X, -q.Y, -q.Z, -q.W);
+        Vector3 v = new(q.X, q.Y, q.Z);
+        float s = v.Length();
+        if (s < 1e-9f)
+            return Vector3.Zero;
+        return v * (2f * Mathf.Atan2(s, q.W) / s);
+    }
+
+    public static bool IsFinite(RigidBody3D body) =>
+        body.GlobalTransform.IsFinite() && body.LinearVelocity.IsFinite() && body.AngularVelocity.IsFinite();
+
+    // Escape hatch: a non-finite body is unrecoverable for the solver and floods the renderer
+    // with !v.is_finite() errors — put it back on its target, motionless. Returns true if reset.
+    public static bool ResetIfNonFinite(RigidBody3D body, Transform3D target, string who)
+    {
+        if (IsFinite(body))
+            return false;
+        body.GlobalTransform = target;
+        body.LinearVelocity = Vector3.Zero;
+        body.AngularVelocity = Vector3.Zero;
+        GD.PushWarning($"{who}: non-finite body state, reset to target");
+        return true;
+    }
+}

scripts/CollapseSpike.cs

@@ -0,0 +1,328 @@
+using System.Collections.Generic;
+using Godot;
+
+// Headless regression for the Stage-1 playtest collapse (2026-10-01). Two families:
+// - Stacking: seated chest overlapping the static table, 50 kg anvil rigid-welded to two
+//   chase hands, the logged grab sequence (L, R two-handed, L release, L re-grab), the real
+//   PropShop with the chest plowing through the props. Did NOT reproduce the NaN — kept as
+//   the guard for the solver-stacking hypothesis.
+// - Yaw drift: head still while the torso yaw target drifts slowly (0.2 deg/s) across the
+//   body's yaw — the actual root cause. The old servo's error quaternion hit W = 1.0000001
+//   there, GetAngle() = 2*acos(W) went NaN, and AngularVelocity += NaN killed the chest.
+// Run: godot-mono --headless --fixed-fps 90 --path . scenes/collapse_spike.tscn
+public partial class CollapseSpike : Node3D
+{
+    private const float PalmGrabOffset = 0.045f;
+    private const float RunawaySpeed = 50f;
+    private const int DriftYawCount = 40;
+    private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
+    private static readonly Vector3 AnvilSize = new(0.28f, 0.16f, 0.13f);
+
+    private struct Scenario
+    {
+        public string Name;
+        public Vector3 Head;
+        public float Duration;
+        public bool Anvil;
+        public bool Table;
+        public bool Props;
+        public bool Sweep;
+        public bool Still;
+        public float Yaw;
+        public float YawDrift;
+    }
+
+    private readonly List<Scenario> _scenarios = new();
+    private int _index = -1;
+    private float _t;
+    private int _step;
+    private Node3D _world;
+    private Node3D _head;
+    private PhysicsTorso _torso;
+    private readonly RigidBody3D[] _hands = new RigidBody3D[2];
+    private readonly Node3D[] _targets = new Node3D[2];
+    private readonly ConeTwistJoint3D[] _welds = new ConeTwistJoint3D[2];
+    private readonly Vector3[] _targetBase = new Vector3[2];
+    private RigidBody3D _anvil;
+    private bool _failed;
+    private float _maxTorsoSpeed;
+    private float _maxTorsoSpin;
+    private float _maxPropSpeed;
+    private float _maxYawErrDeg;
+    private readonly HashSet<string> _done = new();
+    private readonly List<string> _results = new();
+    private int _failCount;
+    private readonly RandomNumberGenerator _rng = new() { Seed = 1302 };
+
+    public override void _Ready()
+    {
+        // Seated head 1.01 m (the playtest calibration); chest center z = head z + 0.12.
+        // Table front edge is z = -0.2, chest half-depth 0.065: overlap starts at head z < -0.255.
+        Add("standing, no table overlap, anvil", new(0.2f, 1.65f, 0.3f), 6f, anvil: true, table: true);
+        Add("seated, chest clear of table, anvil", new(0.2f, 1.01f, 0.05f), 6f, anvil: true, table: true);
+        Add("seated, chest 5 cm into table, no props", new(0.2f, 1.01f, -0.31f), 6f, table: true);
+        Add("seated, chest 15 cm into table, no props", new(0.2f, 1.01f, -0.41f), 6f, table: true);
+        Add("seated, chest 5 cm into table, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, table: true);
+        Add("seated, chest 15 cm into table, anvil", new(0.2f, 1.01f, -0.41f), 6f, anvil: true, table: true);
+        Add("standing, anvil pressed into chest, no table", new(0.2f, 1.65f, 0.3f), 6f, anvil: true);
+        Add("seated, real PropShop, chest sweeps through props, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, props: true, sweep: true);
+        Add("seated, real PropShop, chest sweeps through props, no grabs", new(0.2f, 1.01f, -0.31f), 6f, props: true, sweep: true);
+        for (int i = 0; i < DriftYawCount; i++)
+        {
+            float yaw = -3f + i * 0.1537f;
+            _scenarios.Add(new Scenario
+            {
+                Name = $"seated still, yaw {yaw:F2} drifting 0.2 deg/s, anvil held",
+                Head = new(0.2f, 1.01f, 0.05f), Duration = 2.5f, Anvil = true, Table = true,
+                Still = true, Yaw = yaw, YawDrift = 0.0035f,
+            });
+        }
+        NextScenario();
+    }
+
+    private void Add(string name, Vector3 head, float duration, bool anvil = false, bool table = false, bool props = false, bool sweep = false)
+    {
+        _scenarios.Add(new Scenario { Name = name, Head = head, Duration = duration, Anvil = anvil, Table = table || props, Props = props, Sweep = sweep });
+    }
+
+    private void NextScenario()
+    {
+        if (_world != null)
+        {
+            _world.QueueFree();
+            _world = null;
+        }
+        _index++;
+        if (_index >= _scenarios.Count)
+        {
+            foreach (string line in _results)
+                GD.Print(line);
+            GD.Print($"COLLAPSE: {_scenarios.Count - _failCount}/{_scenarios.Count} scenarios clean");
+            GD.Print($"COLLAPSE: overall {(_failCount == 0 ? "PASS — no NaN/runaway in any scenario" : "FAIL — see above")}");
+            GetTree().Quit();
+            return;
+        }
+        _t = 0f;
+        _step = 0;
+        _failed = false;
+        _maxTorsoSpeed = 0f;
+        _maxTorsoSpin = 0f;
+        _maxPropSpeed = 0f;
+        _maxYawErrDeg = 0f;
+        _done.Clear();
+        _welds[0] = _welds[1] = null;
+        Build(_scenarios[_index]);
+    }
+
+    private void Build(Scenario s)
+    {
+        _world = new Node3D();
+        AddChild(_world);
+
+        StaticBody3D floor = new() { CollisionMask = 0b11101 };
+        floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(10f, 0.1f, 10f) }, Position = new Vector3(0f, -0.05f, 0f) });
+        _world.AddChild(floor);
+
+        if (s.Props)
+        {
+            _world.AddChild(new PropShop());
+        }
+        else if (s.Table)
+        {
+            StaticBody3D table = new() { CollisionMask = 0b11101 };
+            table.AddChild(new CollisionShape3D { Position = new Vector3(0f, 0.725f, -0.6f), Shape = new BoxShape3D { Size = new Vector3(1.4f, 0.05f, 0.8f) } });
+            foreach (Vector3 o in new Vector3[] { new(-0.62f, 0, 0.32f), new(0.62f, 0, 0.32f), new(-0.62f, 0, -0.32f), new(0.62f, 0, -0.32f) })
+                table.AddChild(new CollisionShape3D { Position = new Vector3(o.X, 0.3625f, -0.6f + o.Z), Shape = new BoxShape3D { Size = new Vector3(0.06f, 0.725f, 0.06f) } });
+            _world.AddChild(table);
+        }
+
+        _head = new Node3D { Position = s.Head };
+        _world.AddChild(_head);
+        _torso = new PhysicsTorso { HeadSource = _head };
+        _world.AddChild(_torso);
+        Vector3 chest = s.Head + new Vector3(0f, -0.42f, 0.12f);
+        _torso.Body.GlobalPosition = chest;
+
+        Vector3 anvilPos = s.Table ? new Vector3(s.Head.X, 0.83f, Mathf.Min(chest.Z - 0.2f, -0.3f)) : chest + new Vector3(0f, 0f, -0.2f);
+        _anvil = null;
+        if (s.Props)
+        {
+            foreach (Node n in GetTree().GetNodesInGroup("prop"))
+                if (n is RigidBody3D rb && rb.Name.ToString().StartsWith("Anvil"))
+                    _anvil = rb;
+            anvilPos = _anvil.GlobalPosition;
+            if (!s.Anvil)
+                _anvil = null;
+        }
+        else if (s.Anvil)
+        {
+            _anvil = new RigidBody3D
+            {
+                Name = "Anvil",
+                Mass = 50f,
+                Position = anvilPos,
+                CollisionMask = 0b11101,
+                GravityScale = s.Table ? 1f : 0f,
+                PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.95f },
+            };
+            _anvil.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = AnvilSize } });
+            _world.AddChild(_anvil);
+        }
+
+        for (int h = 0; h < 2; h++)
+        {
+            float side = h == 0 ? -1f : 1f;
+            // Palm offset: left +X, right -X — the hand sits just outside the anvil's X face.
+            Vector3 handPos = anvilPos + new Vector3(side * (AnvilSize.X * 0.5f + HandSize.X * 0.5f + 0.005f), 0f, 0f);
+            _hands[h] = new RigidBody3D
+            {
+                Mass = 0.4f,
+                GravityScale = 0f,
+                ContinuousCd = true,
+                Position = handPos,
+                CollisionLayer = 0b1000,
+                CollisionMask = 0b1001,
+            };
+            _hands[h].AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
+            _world.AddChild(_hands[h]);
+            _targets[h] = new Node3D { Position = handPos };
+            _world.AddChild(_targets[h]);
+            _targetBase[h] = handPos;
+        }
+    }
+
+    public override void _PhysicsProcess(double delta)
+    {
+        if (_world == null)
+            return;
+        float dt = (float)delta;
+        _t += dt;
+        _step++;
+        Scenario s = _scenarios[_index];
+
+        // Controllers: settle, grab L, grab R, haul the anvil toward the chest and down into the
+        // table edge with a wobble, release L, re-grab L (the logged sequence), keep hauling.
+        Vector3 haul = Vector3.Zero;
+        if (_t > 1.0f && !s.Still)
+        {
+            float k = Mathf.Min((_t - 1.0f) / 1.0f, 1f);
+            haul = new Vector3(0f, -0.05f, 0.35f) * k
+                 + new Vector3(0.04f * Mathf.Sin(9f * _t), 0.03f * Mathf.Sin(7f * _t), 0.05f * Mathf.Sin(5f * _t));
+        }
+        for (int h = 0; h < 2; h++)
+            _targets[h].GlobalPosition = _targetBase[h] + haul;
+
+        if (_anvil != null)
+        {
+            Once("grabL", _t > 0.4f, () => Grab(0));
+            Once("grabR", _t > 0.7f, () => Grab(1));
+            if (!s.Still)
+            {
+                Once("releaseL", _t > 2.6f, () => Release(0));
+                Once("regrabL", _t > 3.2f, () => Grab(0));
+            }
+        }
+
+        for (int h = 0; h < 2; h++)
+            PhysicsHands.Drive(_hands[h], _targets[h].GlobalTransform, dt);
+
+        Vector3 sway = s.Still ? Vector3.Zero : s.Sweep
+            ? new Vector3(-0.2f + 0.55f * Mathf.Sin(0.9f * _t), 0f, -0.12f + 0.14f * Mathf.Sin(1.7f * _t))
+            : new Vector3(0.03f * Mathf.Sin(2f * _t), 0f, 0.04f * Mathf.Sin(1.3f * _t));
+        // Still: yaw parked, then drifting slowly across the body's yaw from t = 1 s.
+        // Moving: arbitrary base yaw plus sub-millidegree tracker noise.
+        float yaw = s.Still ? s.Yaw + s.YawDrift * Mathf.Max(_t - 2f, -1f) : 0.37f + 1e-5f * _rng.Randfn();
+        _head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), s.Head + sway);
+
+        if (!_failed)
+            Monitor(s);
+
+        if (_t >= s.Duration)
+        {
+            bool ok = !_failed;
+            if (!ok)
+                _failCount++;
+            if (!ok || !s.Still)
+                _results.Add($"COLLAPSE: [{(ok ? "PASS" : "FAIL")}] {s.Name}: torso |v| max {_maxTorsoSpeed:F2} m/s, |w| max {_maxTorsoSpin:F2} rad/s, props |v| max {_maxPropSpeed:F2} m/s, torso err {_torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition):F3} m");
+            else
+                _results.Add($"COLLAPSE: [PASS] {s.Name}: yaw error max after settle {_maxYawErrDeg:F3} deg");
+            NextScenario();
+        }
+    }
+
+    private void Monitor(Scenario s)
+    {
+        RigidBody3D torso = _torso.Body;
+        Check("torso", torso);
+        Check("handL", _hands[0]);
+        Check("handR", _hands[1]);
+        if (_anvil != null)
+            Check("anvil", _anvil);
+        foreach (Node n in GetTree().GetNodesInGroup("prop"))
+        {
+            if (n is not RigidBody3D prop || prop.IsQueuedForDeletion())
+                continue;
+            Check($"prop '{prop.Name}'", prop);
+            _maxPropSpeed = Mathf.Max(_maxPropSpeed, prop.LinearVelocity.Length());
+            if (prop.LinearVelocity.Length() > RunawaySpeed)
+                Fail($"prop '{prop.Name}' runaway {prop.LinearVelocity.Length():F1} m/s");
+        }
+        if (_failed)
+            return;
+        _maxTorsoSpeed = Mathf.Max(_maxTorsoSpeed, torso.LinearVelocity.Length());
+        _maxTorsoSpin = Mathf.Max(_maxTorsoSpin, torso.AngularVelocity.Length());
+        if (torso.LinearVelocity.Length() > RunawaySpeed || (_anvil != null && _anvil.LinearVelocity.Length() > RunawaySpeed))
+            Fail($"runaway speed (torso {torso.LinearVelocity.Length():F1}, anvil {_anvil?.LinearVelocity.Length() ?? 0f:F1})");
+        if (s.Still && _t > 1.5f)
+        {
+            Vector3 bodyFwd = -torso.GlobalTransform.Basis.Z;
+            Vector3 headFwd = -_head.GlobalTransform.Basis.Z;
+            float err = Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Y, bodyFwd.Dot(headFwd)));
+            _maxYawErrDeg = Mathf.Max(_maxYawErrDeg, Mathf.Abs(err));
+        }
+    }
+
+    private void Check(string name, RigidBody3D body)
+    {
+        if (!body.GlobalTransform.IsFinite() || !body.LinearVelocity.IsFinite() || !body.AngularVelocity.IsFinite())
+            Fail($"{name} non-finite: pos {body.GlobalPosition} v {body.LinearVelocity} w {body.AngularVelocity}");
+    }
+
+    private void Fail(string why)
+    {
+        if (_failed)
+            return;
+        _failed = true;
+        GD.Print($"COLLAPSE: FIRST ANOMALY in '{_scenarios[_index].Name}' t={_t:F3}s step {_step}: {why}");
+    }
+
+    private void Once(string key, bool cond, System.Action action)
+    {
+        if (cond && _done.Add(key))
+            action();
+    }
+
+    private void Grab(int h)
+    {
+        RigidBody3D hand = _hands[h];
+        Vector3 pivot = hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(h == 0 ? PalmGrabOffset : -PalmGrabOffset, 0f, 0f);
+        ConeTwistJoint3D weld = new()
+        {
+            ExcludeNodesFromCollision = true,
+            Position = pivot,
+            NodeA = hand.GetPath(),
+            NodeB = _anvil.GetPath(),
+            SwingSpan = 0f,
+            TwistSpan = 0f,
+            Bias = 0f,
+        };
+        _world.AddChild(weld);
+        _welds[h] = weld;
+    }
+
+    private void Release(int h)
+    {
+        _welds[h]?.QueueFree();
+        _welds[h] = null;
+    }
+}

scripts/CollapseSpike.cs.uid

@@ -0,0 +1 @@
+uid://cecdf0w5cdovg

scripts/PhysicsHands.cs

@@ -88,6 +88,8 @@ public partial class PhysicsHands : Node
     {
         if (body == null)
             return;
+        if (BodyMath.ResetIfNonFinite(body, controller.GlobalTransform, "PhysicsHands"))
+            return;
         int hand = _left == controller ? 0 : 1;
         Vector3 target = controller.GlobalPosition;
 
@@ -121,18 +123,21 @@ public partial class PhysicsHands : Node
                 return;
             }
         }
-        Vector3 desiredVelocity = (target - body.GlobalPosition) / delta;
+        Drive(body, controller.GlobalTransform, delta);
+    }
+
+    // The hand chase itself — public so headless spikes drive their stand-ins with the real code.
+    public static void Drive(RigidBody3D body, Transform3D target, float delta)
+    {
+        Vector3 desiredVelocity = (target.Origin - body.GlobalPosition) / delta;
         Vector3 dv = desiredVelocity - body.LinearVelocity;
         Vector3 force = dv / delta * body.Mass;
         if (force.Length() > HandMaxForce)
             force = force.Normalized() * HandMaxForce;
         body.LinearVelocity += force / body.Mass * delta;
 
-        Basis deltaBasis = controller.GlobalTransform.Basis * body.GlobalTransform.Basis.Inverse();
-        Quaternion q = deltaBasis.GetRotationQuaternion();
-        Vector3 spin = q.GetAxis() * (q.GetAngle() / delta);
-        if (spin.Length() > HandMaxSpin)
-            spin = spin.Normalized() * HandMaxSpin;
-        body.AngularVelocity = spin;
+        Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(),
+            body.GlobalTransform.Basis.GetRotationQuaternion());
+        body.AngularVelocity = (rotErr / delta).LimitLength(HandMaxSpin);
     }
 }

scripts/PhysicsTorso.cs

@@ -92,6 +92,9 @@ public partial class PhysicsTorso : Node
             _mesh.Visible = true;
         }
 
+        if (BodyMath.ResetIfNonFinite(_body, new Transform3D(_torsoBasis, target), "PhysicsTorso"))
+            return;
+
         if (_body.GlobalPosition.DistanceTo(target) > SnapDistance)
         {
             _body.GlobalTransform = new Transform3D(_torsoBasis, target);
@@ -109,13 +112,9 @@ public partial class PhysicsTorso : Node
             force = force.Normalized() * MaxForce;
         _body.LinearVelocity += force / TorsoMass * dt;
 
-        Quaternion qErr = _torsoBasis.GetRotationQuaternion()
-            * _body.GlobalTransform.Basis.GetRotationQuaternion().Inverse();
-        if (qErr.W < 0f)
-            qErr = new Quaternion(-qErr.X, -qErr.Y, -qErr.Z, -qErr.W);
-        float angle = qErr.GetAngle();
-        Vector3 spinAxis = angle > 1e-5f ? qErr.GetAxis() : Vector3.Up;
-        Vector3 spinDesired = spinAxis * Mathf.Min(angle * OrientKp, OrientMaxSpin);
+        Vector3 rotErr = BodyMath.RotationError(_torsoBasis.GetRotationQuaternion(),
+            _body.GlobalTransform.Basis.GetRotationQuaternion());
+        Vector3 spinDesired = (rotErr * OrientKp).LimitLength(OrientMaxSpin);
 
         Vector3 wErr = spinDesired - _body.AngularVelocity;
         float maxDeltaW = OrientMaxAlpha * dt;