From cb518999e546b9e3a5e42c34528ccf6f2c2cdab7 Mon Sep 17 00:00:00 2001 From: Foxyz Date: Thu, 1 Oct 2026 23:55:46 +0300 Subject: [PATCH] Body Stage 1 collapse root-caused + fixed: NaN in torso servo math (GetAngle = 2*acos(W) at W=1.0000001), not solver stacking MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 --- AGENTS.md | 14 +- HANDOFF.md | 48 ++-- research/body-stage1-failure-brief.md | 46 ++++ scenes/collapse_spike.tscn | 6 + scripts/BodyMath.cs | 38 +++ scripts/CollapseSpike.cs | 328 ++++++++++++++++++++++++++ scripts/CollapseSpike.cs.uid | 1 + scripts/PhysicsHands.cs | 19 +- scripts/PhysicsTorso.cs | 13 +- 9 files changed, 466 insertions(+), 47 deletions(-) create mode 100644 scenes/collapse_spike.tscn create mode 100644 scripts/BodyMath.cs create mode 100644 scripts/CollapseSpike.cs create mode 100644 scripts/CollapseSpike.cs.uid diff --git a/AGENTS.md b/AGENTS.md index 7f877f3..59e4722 100644 --- a/AGENTS.md +++ b/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, diff --git a/HANDOFF.md b/HANDOFF.md index 431a67c..49ac199 100644 --- a/HANDOFF.md +++ b/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 diff --git a/research/body-stage1-failure-brief.md b/research/body-stage1-failure-brief.md index 57bf7a3..be4fe7d 100644 --- a/research/body-stage1-failure-brief.md +++ b/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). diff --git a/scenes/collapse_spike.tscn b/scenes/collapse_spike.tscn new file mode 100644 index 0000000..0c23a60 --- /dev/null +++ b/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") diff --git a/scripts/BodyMath.cs b/scripts/BodyMath.cs new file mode 100644 index 0000000..5c2a972 --- /dev/null +++ b/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; + } +} diff --git a/scripts/CollapseSpike.cs b/scripts/CollapseSpike.cs new file mode 100644 index 0000000..680b419 --- /dev/null +++ b/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 _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 _done = new(); + private readonly List _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; + } +} diff --git a/scripts/CollapseSpike.cs.uid b/scripts/CollapseSpike.cs.uid new file mode 100644 index 0000000..71c5d37 --- /dev/null +++ b/scripts/CollapseSpike.cs.uid @@ -0,0 +1 @@ +uid://cecdf0w5cdovg diff --git a/scripts/PhysicsHands.cs b/scripts/PhysicsHands.cs index f41e9a8..f89ecea 100644 --- a/scripts/PhysicsHands.cs +++ b/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); } } diff --git a/scripts/PhysicsTorso.cs b/scripts/PhysicsTorso.cs index 562cbf6..8b0e897 100644 --- a/scripts/PhysicsTorso.cs +++ b/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;