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>

AGENTS.md

@@ -235,18 +235,18 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x
    angular-velocity servo (self-rights, holds yaw), 2 m snap safety, torso-yaw spring moved
    here from BodyRig. Camera stays KINEMATIC — torso chases the head, never the reverse
    (Boneworks sickness lesson). No self-collision with own hands (layer scheme above).
-   **First in-headset session NaN'd the solver** (owner seated: chest overlapped the table;
-   two-handed 50 kg anvil weld pressed into it → `!v.is_finite()` transform flood, "head
-   detaching from the body"). **DO NOT extend the body track until diagnosed** — analysis,
-   hypotheses and the headless-repro plan live in `research/body-stage1-failure-brief.md`
-   (self-contained, forwardable to the owner's external agent). `BodyRig.cs` is
+   **First in-headset session NaN'd the chest — ROOT-CAUSED + FIXED same day**: script
+   math, not the solver (`Quaternion.GetAngle()` = 2·acos(W) → NaN at W = 1.0000001; see
+   `research/body-stage1-failure-brief.md` RESOLUTION). **Never use `GetAngle()/GetAxis()`
+   for servo errors — `BodyMath.RotationError` (atan2) is the rule; every driven body runs
+   `BodyMath.ResetIfNonFinite`.** Regression: `scenes/collapse_spike.tscn` (run headless with
+   `--fixed-fps 90`) must print "COLLAPSE: overall PASS". `BodyRig.cs` is
    VISUALS-only (head cube + 2-bone analytic arms, soft IK, elbow pole,
    reach budget feeding `PhysicsTorso.ReachOffset`). Headless regression:
    `scenes/body_spike.tscn` must print "BODY: overall PASS" (also proves the Box3D pair
    rules: one-way contact, hand-through-chest exclusion, prop contact) — note its repro gap:
    no static-table overlap, no welded heavy props. Stage 2 (physical
-   arm chains + hierarchical per-joint budgets, Nimso/Boneworks tier) is BLOCKED on the
-   Stage-1 fix; design + prior art in `research/vr-body-physics-hugs.md`,
+   arm chains + hierarchical per-joint budgets, Nimso/Boneworks tier) — see item 10; design + prior art in `research/vr-body-physics-hugs.md`,
    stack facts in `research/vr-body-physics-godot.md`.
 9. **Getting a body (rig) — placeholder v1.3 (cube character, agent-built).** `BodyRig.cs`:
    all-BOX primitives (owner preference) — head 0.15³ behind the eyes, slim torso retracted,

HANDOFF.md

@@ -120,10 +120,8 @@ TERMINOLOGY (owner-corrected — use precisely):
   grab/teleport; `IsPanelCollider(node)` identifies interactive colliders for lasers.
 - **TeleportMarker.cs** — `scenes/teleport_marker.tscn`: disc + beam + Label3D, group
   "teleport_marker", StaticBody pick box. `SetNear` hides visuals, `SetHighlighted` swaps colors.
-- **PhysicsTorso.cs** — the player's PHYSICAL CHEST (body-physics Stage 1). **Implemented
-  2026-10-01; FAILED first in-headset playtest (seated chest-vs-table overlap + two-handed
-  50 kg anvil weld → NaN transform flood) — see §5 known issues +**
-  **`research/body-stage1-failure-brief.md` before touching anything here.**
+- **PhysicsTorso.cs** — the player's PHYSICAL CHEST (body-physics Stage 1). Implemented
+  2026-10-01; the first playtest's NaN collapse is root-caused + fixed (servo math, see §5).
   One RigidBody3D box (0.22×0.5×0.13, 30 kg, gravity off, CCD, contact monitor), chase-driven
   toward an HMD-derived target: target = head − 6 cm back + torso-yaw basis·(0, −0.42, +0.06)
   + `ReachOffset` (fed by BodyRig's reach budget). Linear drive = the PhysicsHands pattern at
@@ -216,19 +214,24 @@ Floor StaticBody scans layer 3 (collision_mask 13).
 
 ## 5. Known issues / open questions
 
-- **Body Stage 1 (chest) FAILED in-headset on first playtest (2026-10-01) — DO NOT extend the
-  body track until diagnosed.** Owner report: "it quickly collapsed with my head detaching
-  from the body"; screenshot = prop table chaos. Log evidence: session calibrated at head
-  height **1.01 m (seated)** → chest box overlaps the table; the NaN flood
-  (`!v.is_finite()` at `instance_set_transform`, 22.8k lines) starts right after
-  **"grabbed 'Anvil · 50 kg' (50 kg, left, two-handed)"** — i.e. chest-vs-statics overlap +
-  50 kg prop rigid-welded to both 90 N hands pressed into that mess. Headless body spike
-  passes (it lacks static-table overlap + welded heavy props — that's the repro gap).
-  Full self-contained analysis for the owner's external agent:
-  **`research/body-stage1-failure-brief.md`** (evidence chain, hypotheses H1–H3, mitigation
-  candidates M1–M5, headless-repro plan). Working suspicion: the documented
-  "script velocity SETs + reaction forces = limit cycle" lesson (grab v5.0) resurfacing at
-  body scale — 30 kg velocity-write servo braced against statics.
+- **Body Stage 1 chest collapse — ROOT-CAUSED + FIXED (2026-10-01), awaiting owner re-playtest.**
+  The first in-headset session NaN'd the chest ("head detaching from the body"). Cause was
+  SCRIPT MATH, not the solver: the orientation servo used `Quaternion.GetAngle()` = `2*acos(W)`
+  (Godot C# `Mathf.Acos` is unclamped) → NaN when rounding put W at 1.0000001, which a seated
+  still player reaches because `GetAxis()` (raw xyz when W > 1−1e-6) collapsed the servo gain
+  inside 0.16° and parked the body where a slowly drifting target crosses it. Fixed with
+  `BodyMath.RotationError` (atan2-based) in torso + hands, plus `BodyMath.ResetIfNonFinite`
+  escape hatch on every driven body. Deterministic repro + regression:
+  `scenes/collapse_spike.tscn` (old code 42/49, fixed 49/49). Solver stacking (chest in
+  static table + two-hand-welded 50 kg anvil) was tested and is stable. Full record:
+  `research/body-stage1-failure-brief.md` (RESOLUTION section). **RULE: never use
+  `Quaternion.GetAngle()/GetAxis()` for servo errors — use `BodyMath.RotationError`.**
+- **Seated calibration** — BodyRig calibrates proportions from the head height at first
+  frame; seated (1.01 m) clamps arm length to the 0.50 m minimum, so normal reaches saturate
+  `ReachOffset` and lean the chest into the table (stable, but wrong proportions). Needs a
+  standing calibration step or a seated mode — owner's call.
+- **Chest plows table props** when seated at the table (props thrown up to ~3.8 m/s by the
+  2400 N chest). Honest physics; tune by feel.
 - **AnchorMoveMode NEEDS FIXING (owner playtest report, 2026-10-01)** — two-hand turning
   is NOT working as intended (not 2-anchor based) and simultaneous two-hand movement acts
   weird. NOTE before fixing: owner is considering replacing grab-locomotion with physics
@@ -311,15 +314,8 @@ Floor StaticBody scans layer 3 (collision_mask 13).
 
 ## 7. Next steps (in order)
 
-1. **Diagnose + fix the body Stage-1 collapse (BLOCKS the body track)** — first playtest of
-   the physical chest NaN'd the solver (seated chest-in-table overlap + two-handed 50 kg
-   anvil weld pressed into it; "head detaching from the body"). All context, hypotheses,
-   mitigation candidates and the headless-repro plan are in
-   `research/body-stage1-failure-brief.md` — written self-contained so the owner can forward
-   it to their external agent verbatim. First move per the brief: extend `body_spike` with
-   the exact cocktail (chest target overlapping a static slab + two welded 90 N hands
-   dragging a 50 kg box into the chest) and get a deterministic NaN repro before touching
-   PhysicsTorso.cs. Owner re-playtests feel only after the fix.
+1. **Owner re-playtest of the body (chest fix + Stage 2 arms)** — the Stage-1 collapse is
+   fixed (`collapse_spike` 49/49); feel is unvalidated in-headset. Read the log after.
 2. **Locomotion decision** — owner report 2026-10-01: two-hand anchor turning broken (not
    2-anchor based), simultaneous two-hand movement acts weird, teleport + lasers broken
    (experiments, not current issues). Owner's candidate replacement: physics thumbstick

research/body-stage1-failure-brief.md

@@ -1,5 +1,8 @@
 # Body Stage 1 failure brief — chest collapse in-headset (2026-10-01)
 
+**STATUS: RESOLVED — root cause = NaN in the torso orientation servo math (not the solver).
+See the RESOLUTION section at the end. The hypotheses below are kept as the record.**
+
 SELF-CONTAINED brief for an external agent (zero project context assumed). Godot 4.7.2 mono,
 C#, PCVR at 90 Hz (physics tick = 90), physics engine = **Box3D** (bearlikelion's
 `godot-box3d` GDExtension — a full PhysicsServer3D replacement; locally patched so Godot's
@@ -127,3 +130,46 @@ the prop table in chaos (props piled/thrown, panel knobs displaced). Log
 
 Owner directive: painlessness bar = no multi-iteration fundamental sagas. Deliver the
 analysis, the repro, and the smallest honest fix — the owner playtests feel afterwards.
+
+## RESOLUTION (2026-10-01, same day) — root cause found, deterministic repro, fixed
+
+**The NaN was script math, not the solver.** H1–H3 were all tested headless and did NOT
+reproduce (`scenes/collapse_spike.tscn`, stacking family: chest 5/15 cm into the static
+table, 50 kg anvil rigid-welded to two 90 N chase hands, the exact logged grab sequence,
+the real PropShop with the chest plowing through props — all finite).
+
+Root cause, verified step by step:
+1. The orientation servo computed `qErr.GetAngle()` = `2*acos(W)`. Godot C#'s `Mathf.Acos`
+   is UNCLAMPED (docs: input "must be" in [-1, 1]); when float rounding puts the error
+   quaternion's W at **1.0000001**, the angle is NaN. `Mathf.Min(NaN, cap)` passes NaN
+   through, the slew clamp's `wErr.Length() > max` is false for NaN, and
+   `AngularVelocity += NaN` goes straight into Box3D (`b3Body_SetAngularVelocity` only
+   asserts validity in debug builds). The chest transform goes NaN → renderer flood →
+   arm visuals (derived from the chest pose) vanish with it → "head detached from body".
+2. Why it needed a seated, still player: `Quaternion.GetAxis()` returns the RAW
+   un-normalized xyz once W > 1 − 1e-6 (angle < ~0.16°), so the servo's gain collapsed
+   ~1000× inside that band — the body parked a fraction of a degree off target. The
+   rounding hazard (error < ~3e-4 rad) is only reached when the yaw target then DRIFTS
+   SLOWLY across that parked yaw — exactly a seated player holding still (the 50 kg anvil
+   welded to both hands can't move: 2×90 N < 0.95×490 N friction, hence the logged
+   0.01 m/s releases). Normal motion crosses the band too fast. The hands and anvil stayed
+   finite (they kept grabbing after the flood), consistent with a torso-only NaN.
+3. Deterministic repro: head still at 40 yaws, target drifting 0.2°/s → **7/40 scenarios
+   NaN the chest** on the old code (W = 1.0000001 at the crossing tick, body w = 0).
+
+Fix (`scripts/BodyMath.cs`): `RotationError()` = shortest-arc axis·angle via
+`2*atan2(|xyz|, W)` with a properly normalized axis — exact near zero, can't leave its
+domain. Used by the torso servo AND the hand rotation lock (hands were not observed to NaN
+— their Basis-product path measured 0/200k — but shared the gain collapse). Escape hatch:
+`BodyMath.ResetIfNonFinite()` on every driven body (torso, hands) — any future non-finite
+state resets that body onto its target with a warning instead of poisoning the renderer.
+After the fix: collapse spike 49/49 clean ×3, yaw error under slow drift 0.026° (was a
+0.16° stall); physics/weld/grab/body spikes all PASS ×3.
+
+Mitigations M1–M4 were NOT needed for the NaN. Side observations kept for the owner:
+- The prop "table chaos" is real and separate: a seated chest sweeping the table plows
+  props off at up to ~3.8 m/s (2400 N body) — honest physics, tune by feel if unwanted.
+- Seated calibration: head height 1.01 m clamps arm length to the 0.50 m minimum, so
+  reaching saturates `ReachOffset` (0.25 m) and leans the chest INTO the table. The chest
+  then rests saturated on the table edge — stable in every repro, but a calibration that
+  assumes standing height is the deeper issue (see HANDOFF known issues).

scenes/collapse_spike.tscn

@@ -0,0 +1,6 @@
+[gd_scene load_steps=2 format=3]
+
+[ext_resource type="Script" path="res://scripts/CollapseSpike.cs" id="1_spike"]
+
+[node name="CollapseSpike" type="Node3D"]
+script = ExtResource("1_spike")

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;