diff --git a/AGENTS.md b/AGENTS.md index 59e4722..1cfc05c 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -215,9 +215,10 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x **Grab v5.3.1 VALIDATED in-headset (owner: "it just works") — grab saga closed.** `PhysicsHands.cs`: physical palm bodies (0.4 kg, 90 N chase, rotation lock, stuck-safety snap — SUSPENDED for a hand while it grips, HAND ghosts > 10 cm lag). Collision layers: - **1 = world+props, 2 = UI, 3 = player body (PhysicsTorso chest), 4 = player hands** — - chest↔own hands excluded BOTH ways (no self-collision, Nimso/VRChat precedent); props - mask 1|3|4, floor/table mask 1|3|4, hands 4/1|4, chest 3/1|3; panel stays 2/0 (props pass + **1 = world+props, 2 = UI, 3 = player body (PhysicsTorso chest), 4 = player hands, + 5 = player arms (PhysicsArms segments)** — no self-collision between own body parts + (Nimso/VRChat precedent); props/floor/table mask 1|3|4|5, hands 4/1|4, chest 3/1|3, + arm segments 5/1; panel stays 2/0 (props pass through even when shown); layers live on CollisionObject3D, NOT CollisionShape3D. Panel = prop-UI prototype (ax_button summon, contact-press knobs, Reset). Laser/distant grab REMOVED (user never knew it existed); @@ -241,14 +242,16 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x 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: + VISUALS-only (head cube + arm boxes drawn through the physical elbow; IK + reach budget + moved to `PhysicsArms`, item 10). 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) — 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`: +9. **Getting a body (rig) — placeholder v1.3 (cube character, agent-built).** (The IK/yaw/reach + math described here now lives in `PhysicsArms.cs` / `PhysicsTorso.cs`; `BodyRig.cs` only + draws.) Original notes: all-BOX primitives (owner preference) — head 0.15³ behind the eyes, slim torso retracted, wrist-anchored 2-bone arms. **Soft IK applied**: shoulder→hand distance clamped to reach×0.98 before the solve (the straight-arm singularity was the overextension shake; if it @@ -263,3 +266,18 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x the head/neck look override exists. **HANDOFF.md at project root is the takeover document** (constant table verified against code — code wins on drift); keep it updated when systems change. +10. **Body physics Stage 2 — PHYSICAL ARMS, implemented 2026-10-02, awaiting owner playtest.** + `PhysicsArms.cs`: per side an upper-arm + forearm RigidBody3D (2.0 / 1.2 kg, layer 5) that + PD-chase their analytic 2-bone IK pose, solved each physics tick from the PHYSICAL chest + shoulders to the PHYSICAL hands. **No joints** — Box3D has no usable 3-DOF shoulder/wrist + joint (PinJoint's hidden identity spring, no Generic6DOF) and this file's architecture + forbids joints between tracked bodies; the research's hinge-motor chain is therefore + deferred, chase segments are the Box3D-honest version. Hierarchical budgets (Nimso muscle + model) from a 60 kg body-mass stat: hand 90 N ≺ forearm 180 N ≺ upper arm 360 N ≺ chest + 2400 N. What it gives: arms that push props, rest on / get blocked by geometry (no + tunneling), and draw honestly (BodyRig's elbow = the physical elbow). What it does NOT do + yet (Stage 2b candidates, owner feel decides): a blocked forearm does not hold the HAND + back (hands still chase the controllers independently), and prop/NPC forces on the arm + don't flow into the chest. Regression: `scenes/arm_spike.tscn` "ARM: overall PASS"; + the full body also runs inside `scenes/collapse_spike.tscn`. Design + prior art: + `research/vr-body-physics-hugs.md` §5–7, `research/vr-body-physics-godot.md`. diff --git a/HANDOFF.md b/HANDOFF.md index 49ac199..686a7a7 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -30,9 +30,12 @@ TERMINOLOGY (owner-corrected — use precisely): RigidBody3D/StaticBody3D/CharacterBody3D so the flip is lossless. - Runtime stack: **WiVRN + WayVR** on Linux. Headset + runtime support hand tracking (treat as available). - Renderer **Forward+** (Mobile is the docs' VR recommendation; deferred until profiling justifies). -- Build: `dotnet build`. Headless validation — physics: `godot-mono --headless --path . scenes/physics_spike.tscn` - must print **"SPIKE: overall PASS"**; grab: `godot-mono --headless --path . scenes/grab_spike.tscn` - must print **"GRAB: overall PASS"**. Both PASS as of this handoff. Headless asset import: +- Build: `dotnet build`. Headless validation: `godot-mono --headless --fixed-fps 90 --path . scenes/.tscn` + (`--fixed-fps 90` runs physics faster than real time at the real 90 Hz tick — the 49-scenario + collapse spike takes seconds instead of minutes). Spikes and their pass lines: + `physics_spike` "SPIKE: overall PASS", `weld_spike` "WELD: …", `grab_spike` "GRAB: …", + `body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS". + All six PASS 3× as of this handoff. Headless asset import: `godot-mono --headless --import`. - Engine logs: `~/.local/share/godot/app_userdata/VR Messs/logs/godot.log` — **read after user playtests instead of guessing**. @@ -124,7 +127,7 @@ TERMINOLOGY (owner-corrected — use precisely): 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 + + `ReachOffset` (fed by PhysicsArms' reach budget). Linear drive = the PhysicsHands pattern at body mass (force-clamped PD, `MaxForce` 2400 N ⇒ 80 m/s², `MaxSpeed` 6 m/s). Orientation = slewed angular-velocity servo toward the yaw target (Kp 20 rad/s per rad, spin cap 10 rad/s, alpha cap 150 rad/s²) — self-rights when shoved, holds yaw. `SnapDistance` 2 m snap safety @@ -136,11 +139,39 @@ TERMINOLOGY (owner-corrected — use precisely): `scenes/body_spike.tscn` must print **"BODY: overall PASS"** (pair-rule proofs: one-way floor contact, hand-through-chest exclusion, prop-contact two-way; track/yaw/impact/snap/ self-right asserts). -- **BodyRig.cs** — placeholder rig VISUALS only now (v1.3): head cube (HMD-driven), 2-bone - analytic arms (soft IK 0.98, pole as before), shoulders derived from the PhysicsTorso BODY - pose (+ ShoulderRise 0.25, ± body X). The torso mesh and the torso yaw spring moved into - PhysicsTorso; BodyRig computes the reach budget (hysteresis unchanged) and feeds it to - `PhysicsTorso.ReachOffset`. Hidden until calibrated. +- **PhysicsArms.cs** — body-physics **Stage 2: physical arm segments** (2026-10-02). Per side an + upper-arm (2.0 kg) and forearm (1.2 kg) RigidBody3D box, gravity off, CCD, **layer 5 (player + arms) / mask 1** — collide with world + props, never with the own chest, hands or each other. + NO joints (Box3D has no usable 3-DOF shoulder/wrist joint — PinJoint's identity spring, no + Generic6DOF — and AGENTS.md forbids joints between tracked bodies): each segment PD-chases + its analytic 2-bone IK pose, solved EVERY PHYSICS TICK from the PHYSICAL chest shoulders to + the PHYSICAL hand bodies (IK moved here from BodyRig: soft IK 0.98, torso-frame pole with + the shoulder→hand axis projected out, hand-forward push, exp-smoothed). **Hierarchical + budgets** (Nimso muscle model) from `BodyMassStat` 60 kg: hand 90 N ≺ forearm 180 N ≺ upper + arm 360 N ≺ chest 2400 N. Orientation = slewed angular-velocity servo via + `BodyMath.RotationError`; segment roll follows the body's current X axis (no flips). + Safety: 1 m snap (teleports), `ResetIfNonFinite`. Also owns calibration (arm lengths + + shoulder width from first head height > 0.5 m; segments spawn then) and the REACH BUDGET + (torso lean toward overreach → `PhysicsTorso.ReachOffset`, hysteresis unchanged). Exposes + `Shoulder/Elbow/Wrist/ElbowTarget/WristTarget(side)`; `Elbow` = where the two physical + segments meet. Spike: `scenes/arm_spike.tscn` (track gap, fast swing, arm pushes a prop + with the hand clear, forearm blocked by a bar without tunneling + quiet while pinned, + teleport snap, NaN-injection reset, chest undisturbed by own arms). +- **BodyRig.cs** — placeholder rig VISUALS only: head cube (HMD-driven) + arm boxes drawn + shoulder (PhysicsArms, from the chest body) → PHYSICAL elbow → hand wrist, stretched to stay + connected (an obstructed segment shows as a displaced elbow). Hidden until PhysicsArms + calibrates. IK, calibration and reach budget moved to PhysicsArms; torso mesh + yaw spring + live in PhysicsTorso. +- **BodyMath.cs** — shared servo math. `RotationError(target, current)` = shortest-arc + axis·angle via `2·atan2(|xyz|, W)` — **the only allowed way to compute servo rotation + errors** (`Quaternion.GetAngle()` NaNs at W = 1.0000001, `GetAxis()` is un-normalized near + identity — the Stage-1 collapse). `ResetIfNonFinite(body, target, who)` — escape hatch run + by every driven body (hands, chest, arm segments). +- **CollapseSpike.cs** + `scenes/collapse_spike.tscn` — Stage-1 collapse regression with the + full body (chest + arms + hands): 9 solver-stacking scenarios (seated chest in the static + table, two-hand-welded 50 kg anvil + the logged grab sequence, real PropShop sweep) + 40 + slow-yaw-drift scenarios (the root-cause repro: old code NaN'd 7/40). Must print + "COLLAPSE: overall PASS" (49/49). - **GrabSpike.cs** + `scenes/grab_spike.tscn` — headless grab regression (v5.3): a hand stand-in (0.4 kg body, `HandMaxForce` 90 N chase) grabs a 1 kg prop via ConeTwist weld (server type verified, configure-before-AddChild); drag 2 m + 360° fast-spin sub-phase — asserts RIGID @@ -175,8 +206,9 @@ leaving the standard RigidBody3D stack). Scene wiring (`scenes/main.tscn`): Main → WorldEnvironment, Sun, Floor, XROrigin3D (XRCamera3D, LeftHand, RightHand, WorldPanel), SpawnPoint, 3 TeleportMarkers, Locomotion, AnchorMoveMode, -PropShop, PropGrabber, PhysicsHands, PhysicsTorso, BodyRig, FadeLayer/Fade. Action map: `openxr_action_map.tres`. -Floor StaticBody scans layer 3 (collision_mask 13). +PropShop, PropGrabber, PhysicsHands, PhysicsTorso, PhysicsArms, BodyRig, FadeLayer/Fade +(physics order matters: hands → chest → arms, so the arm IK reads this tick's bodies). Action map: `openxr_action_map.tres`. +Floor StaticBody mask 1|3|4|5 (collision_mask 29). ## 4. Architecture decisions that must not regress @@ -201,10 +233,14 @@ Floor StaticBody scans layer 3 (collision_mask 13). as fallback (git history). Stink-O's box3d-godot (custom components, more joint types) is the plan-B. - **Collision layers**: 1 = world, 2 = UI. Panel/knob bodies are layer 2 mask 0 (props pass through - the menu); **1 = world+props, 2 = UI, 3 = player body (chest), 4 = player hands** — chest↔own - hands excluded BOTH ways (no self-collision; props mask 1|3|4, floor mask 1|3|4, hands 4/1|4, - chest 3/1|3). Layers/masks live on `CollisionObject3D`, never `CollisionShape3D`. The + the menu); **1 = world+props, 2 = UI, 3 = player body (chest), 4 = player hands, 5 = player arms** — + no self-collision between any own body parts (props/table/floor mask 1|3|4|5, hands 4/1|4, + chest 3/1|3, arm segments 5/1). Layers/masks live on `CollisionObject3D`, never `CollisionShape3D`. The body_spike proves the pair rules on Box3D. +- **Servo rotation errors only via `BodyMath.RotationError`** — never `Quaternion.GetAngle()` + / `GetAxis()` (NaN at W = 1.0000001 killed the chest in the first body playtest). +- **No joints between player body parts** — arms are chase segments; Box3D can't do a + shoulder/wrist joint cleanly (see PhysicsArms). - **Ghosts belong to hands, not props** — the whole arm gets a ghost later. - **Standard-RigidBody3D-only** for scene objects and tracked bodies, so the Jolt fallback stays lossless. - **`pose_recentered` is never used for placement** — WiVRN fires it continuously (~90 Hz); @@ -226,6 +262,13 @@ Floor StaticBody scans layer 3 (collision_mask 13). 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`.** +- **Body Stage 2 (physical arms) — unvalidated in-headset.** Things to watch in the first + playtest: elbow drawn displaced when a segment is blocked (that's the honest physics view), + arms sweeping props when seated at the table, any stutter at fast swings (spike: ≤ 6 cm + joint lag at 2 Hz × 0.4 m). Known gaps by design (Stage 2b, feel-driven): a blocked forearm + does NOT hold the hand back, and forces on the arms don't reach the chest — both need a + coupling decision (script-side feedback vs joints) the owner should weigh in on after + feeling 2a. - **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 @@ -293,18 +336,25 @@ Floor StaticBody scans layer 3 (collision_mask 13). | `GrabRadius` / `CenterGrabRadius` | PropGrabber.cs | 0.16 m / 0.12 m | palm-point / hand-center grab range | | `PalmGrabOffset` | PropGrabber.cs | ±0.045 m along hand +X (left +, right −) | grab point on the palm (taste choice) | | Grip/trigger thresholds | several files | 0.6 | polled Float edges | -| `SoftIKFactor` | BodyRig.cs | 0.98 | shoulder→hand distance clamp before IK solve (lower toward 0.95 if shake persists) | -| `PoleHandWeight` / `PoleSmoothRate` | BodyRig.cs | 0.25 / 10 | elbow pole: hand-forward push / exp smoothing | +| `SoftIKFactor` | PhysicsArms.cs | 0.98 | shoulder→hand distance clamp before IK solve (lower toward 0.95 if shake persists) | +| `PoleHandWeight` / `PoleSmoothRate` | PhysicsArms.cs | 0.25 / 10 | elbow pole: hand-forward push / exp smoothing | | `TorsoYawHandsWeight` / `TorsoYawForwardness` / `TorsoYawSmoothRate` | PhysicsTorso.cs | 0.6 / 0.3 m / 12 | torso yaw spring: hands blend / forwardness normalizer / exp smoothing (≈200 ms) | | `TorsoMass` / `MaxForce` / `MaxSpeed` | PhysicsTorso.cs | 30 kg / 2400 N / 6 m/s | chest chase budget (force-clamped PD) | | `OrientKp` / `OrientMaxSpin` / `OrientMaxAlpha` | PhysicsTorso.cs | 20 / 10 rad/s / 150 rad/s² | chest orientation servo (yaw hold + self-right) | | `SnapDistance` | PhysicsTorso.cs | 2 m | chest teleport snap safety | -| `armTotal` | BodyRig.cs | clamp(0.46×headHeight, 0.5, 0.72) | total arm length, 50/50 split | -| `ReachPushSpeed` / `ReachDecaySpeed` | BodyRig.cs | 0.8 / 0.4 m/s | torso lean toward / back from overreach | -| `ReachDecayFactor` | BodyRig.cs | 0.92 | hysteresis band (decay only below 92% of reach) | -| `ReachBudgetMax` | BodyRig.cs | 0.25 m | horizontal torso offset clamp | -| `ReachVerticalMin/Max` | BodyRig.cs | −0.05 / +0.12 m | vertical torso offset clamp (stops upward oscillation) | -| `HeadBackOffset` / `TorsoBackOffset` / `TorsoDrop` | BodyRig.cs | 0.06 / 0.06 / 0.42 m | keeps boxes out of the view | +| `armTotal` | PhysicsArms.cs | clamp(0.46×headHeight, 0.5, 0.72) | total arm length, 50/50 split | +| `ReachPushSpeed` / `ReachDecaySpeed` | PhysicsArms.cs | 0.8 / 0.4 m/s | torso lean toward / back from overreach | +| `ReachDecayFactor` | PhysicsArms.cs | 0.92 | hysteresis band (decay only below 92% of reach) | +| `ReachBudgetMax` | PhysicsArms.cs | 0.25 m | horizontal torso offset clamp | +| `ReachVerticalMin/Max` | PhysicsArms.cs | −0.05 / +0.12 m | vertical torso offset clamp (stops upward oscillation) | +| `HeadBackOffset` | BodyRig.cs / PhysicsTorso.cs | 0.06 m | keeps the head cube out of the view | +| `TorsoBackOffset` / `TorsoDrop` | PhysicsTorso.cs | 0.06 / 0.42 m | chest target below/behind the head | +| `BodyMassStat` | PhysicsArms.cs | 60 kg | scales the arm budgets (N/kg below) | +| `ForearmForcePerKg` / `UpperArmForcePerKg` | PhysicsArms.cs | 3 / 6 N/kg (180 / 360 N) | arm segment chase budgets (hand 90 ≺ fore ≺ upper ≺ chest 2400) | +| `ForearmMass` / `UpperArmMass` | PhysicsArms.cs | 1.2 / 2.0 kg | arm segment masses | +| `SegmentMaxSpin` / `ForearmMaxAlpha` / `UpperArmMaxAlpha` | PhysicsArms.cs | 40 rad/s / 1500 / 1000 rad/s² | arm segment orientation servo caps | +| `SnapDistance` | PhysicsArms.cs | 1 m | arm segment teleport snap | +| `ShoulderRise` | PhysicsArms.cs | 0.25 m | shoulder height above the chest body center | | `LaserMinDistance` | Locomotion.cs | 0.35 m | laser hidden nearer than this (tip touch instead) | | `MarkerNearDistance` | Locomotion.cs | 1.2 m | near markers hide + become untargetable | | Fade times | Locomotion.cs | 0.12 s in / 0.25 s out | teleport fade | @@ -324,8 +374,9 @@ Floor StaticBody scans layer 3 (collision_mask 13). must decouple first if deleted). 3. **Grab points on special objects** — grab is palm-proximity to the prop origin today; special objects may want authored grab anchors (see `research/vr-grabbing.md` taxonomy). -4. **Physical torso/head capsules** — folded into the body-physics track (item 1): the torso - chase body IS the capsule replacement; head/body colliders join it in the same stage. +4. **Body Stage 2b (feel-driven)** — after the owner feels the 2a arms: couple obstruction + back into the hands (blocked forearm limits the hand) and arm forces into the chest; then + head collider. Decide calibration for seated sessions (see §5). 5. **Hand tracking spike** — `XRHandTracker` through WiVRN; controller tip and fingertip must feed the same cursor concept (interaction must not assume controllers). 6. **Neck/head look system** — `LookAtModifier3D` head→neck distribution (needs a real skeleton). diff --git a/scenes/arm_spike.tscn b/scenes/arm_spike.tscn new file mode 100644 index 0000000..0e1c0c2 --- /dev/null +++ b/scenes/arm_spike.tscn @@ -0,0 +1,6 @@ +[gd_scene load_steps=2 format=3] + +[ext_resource type="Script" path="res://scripts/ArmSpike.cs" id="1_spike"] + +[node name="ArmSpike" type="Node3D"] +script = ExtResource("1_spike") diff --git a/scenes/main.tscn b/scenes/main.tscn index e9b753c..f0b8a15 100644 --- a/scenes/main.tscn +++ b/scenes/main.tscn @@ -1,4 +1,4 @@ -[gd_scene load_steps=20 format=3] +[gd_scene load_steps=21 format=3] [ext_resource type="Script" path="res://scripts/Main.cs" id="1_main"] [ext_resource type="PackedScene" path="res://scenes/teleport_marker.tscn" id="2_marker"] @@ -10,6 +10,7 @@ [ext_resource type="Script" path="res://scripts/PhysicsHands.cs" id="8_physicshands"] [ext_resource type="Script" path="res://scripts/BodyRig.cs" id="9_bodyrig"] [ext_resource type="Script" path="res://scripts/PhysicsTorso.cs" id="10_physicsTorso"] +[ext_resource type="Script" path="res://scripts/PhysicsArms.cs" id="11_physicsArms"] [sub_resource type="ProceduralSkyMaterial" id="ProceduralSkyMaterial_sky"] sky_horizon_color = Color(0.66, 0.72, 0.81, 1) @@ -56,7 +57,7 @@ shadow_bias = 0.04 shadow_normal_bias = 0.5 [node name="Floor" type="StaticBody3D" parent="."] -collision_mask = 13 +collision_mask = 29 [node name="FloorShape" type="CollisionShape3D" parent="Floor"] transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, -0.05, 0) @@ -118,6 +119,9 @@ script = ExtResource("8_physicshands") [node name="PhysicsTorso" type="Node" parent="."] script = ExtResource("10_physicsTorso") +[node name="PhysicsArms" type="Node" parent="."] +script = ExtResource("11_physicsArms") + [node name="BodyRig" type="Node3D" parent="."] script = ExtResource("9_bodyrig") diff --git a/scripts/ArmSpike.cs b/scripts/ArmSpike.cs new file mode 100644 index 0000000..f0603c5 --- /dev/null +++ b/scripts/ArmSpike.cs @@ -0,0 +1,277 @@ +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 _names = new(); + private readonly List _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(); + } +} diff --git a/scripts/ArmSpike.cs.uid b/scripts/ArmSpike.cs.uid new file mode 100644 index 0000000..c7aa80b --- /dev/null +++ b/scripts/ArmSpike.cs.uid @@ -0,0 +1 @@ +uid://ksf2lcey06ag diff --git a/scripts/BodyRig.cs b/scripts/BodyRig.cs index bdee442..c24d770 100644 --- a/scripts/BodyRig.cs +++ b/scripts/BodyRig.cs @@ -1,52 +1,39 @@ using Godot; +// Placeholder rig VISUALS only: head cube on the HMD, arm boxes drawn from the physical body +// (PhysicsTorso shoulders → PhysicsArms elbow → hand wrist). Arm IK, calibration and the reach +// budget live in PhysicsArms; the torso mesh lives in PhysicsTorso. public partial class BodyRig : Node3D { private const float HeadBackOffset = 0.06f; private const float HeadSize = 0.15f; - private const float ShoulderRise = 0.25f; - private const float ReachBudgetMax = 0.25f; - private const float ReachVerticalMin = -0.05f; - private const float ReachVerticalMax = 0.12f; - private const float ReachPushSpeed = 0.8f; - private const float ReachDecaySpeed = 0.4f; - private const float ReachDecayFactor = 0.92f; private const float WristOffset = 0.06f; - private const float SoftIKFactor = 0.98f; - private const float PoleHandWeight = 0.25f; - private const float PoleSmoothRate = 10f; private XRCamera3D _camera; private XRController3D _left; private XRController3D _right; - private PhysicsTorso _torso; + private PhysicsArms _arms; private MeshInstance3D _headMesh; private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2]; private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2]; - private bool _calibrated; - private float _upperLen; - private float _foreLen; - private float _shoulderHalfWidth; - private Vector3 _torsoOffset = Vector3.Zero; + private bool _sized; private Basis _yawBasis = Basis.Identity; - private Basis _torsoBasis = Basis.Identity; - private readonly Vector3[] _lastPole = new Vector3[2]; public override void _Ready() { _camera = GetNode("../XROrigin3D/XRCamera3D"); _left = GetNode("../XROrigin3D/LeftHand"); _right = GetNode("../XROrigin3D/RightHand"); - _torso = GetNode("../PhysicsTorso"); + _arms = GetNode("../PhysicsArms"); _headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f)); - _upperArmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f)); - _upperArmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f)); - _forearmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f)); - _forearmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f)); - + for (int side = 0; side < 2; side++) + { + _upperArmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f)); + _forearmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f)); + } SetRigVisible(false); } @@ -70,44 +57,37 @@ public partial class BodyRig : Node3D public override void _Process(double delta) { UpdateYawBasis(); - float dt = (float)delta; - - if (!_calibrated) - { - float headHeight = _camera.GlobalPosition.Y; - if (headHeight <= 0.5f) - return; - Calibrate(headHeight); - } - if (_torso.Body == null) + if (!_arms.Calibrated || _arms.UpperArm(0) == null) return; + if (!_sized) + { + _sized = true; + for (int side = 0; side < 2; side++) + { + ((BoxMesh)_upperArmMeshes[side].Mesh).Size = new Vector3(0.07f, _arms.UpperLen, 0.07f); + ((BoxMesh)_forearmMeshes[side].Mesh).Size = new Vector3(0.06f, _arms.ForeLen, 0.06f); + } + SetRigVisible(true); + } Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset; _headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos); - _torsoBasis = _torso.Body.GlobalTransform.Basis; - Vector3 torsoCenter = _torso.Body.GlobalPosition; + PlaceArm(0, PhysicsHands.GetBody(_left)); + PlaceArm(1, PhysicsHands.GetBody(_right)); + } - Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise; - Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth; - Vector3 rightShoulder = topCenter + _torsoBasis.X * _shoulderHalfWidth; - - Vector3 push = SolveArm(0, leftShoulder, -_torsoBasis.X, PhysicsHands.GetBody(_left), _upperArmMeshes[0], _forearmMeshes[0], dt, out bool decayLeft); - push += SolveArm(1, rightShoulder, _torsoBasis.X, PhysicsHands.GetBody(_right), _upperArmMeshes[1], _forearmMeshes[1], dt, out bool decayRight); - - if (push != Vector3.Zero) - { - _torsoOffset += push * (dt * ReachPushSpeed); - } - else if (decayLeft || decayRight) - { - _torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt); - } - - Vector3 horizontal = new(_torsoOffset.X, 0f, _torsoOffset.Z); - horizontal = horizontal.LimitLength(ReachBudgetMax); - _torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z); - _torso.ReachOffset = _torsoOffset; + private void PlaceArm(int side, RigidBody3D hand) + { + Vector3 shoulder = _arms.Shoulder(side); + Vector3 elbow = _arms.Elbow(side); + Vector3 wrist = hand != null + ? hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset) + : _arms.Wrist(side); + if (!shoulder.IsFinite() || !elbow.IsFinite() || !wrist.IsFinite()) + return; + PlaceSegment(_upperArmMeshes[side], shoulder, elbow); + PlaceSegment(_forearmMeshes[side], elbow, wrist); } private void UpdateYawBasis() @@ -119,97 +99,21 @@ public partial class BodyRig : Node3D _yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up); } - private void Calibrate(float headHeight) - { - _calibrated = true; - float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f); - _upperLen = 0.5f * armTotal; - _foreLen = 0.5f * armTotal; - _shoulderHalfWidth = 0.16f + headHeight * 0.02f; - - ((BoxMesh)_upperArmMeshes[0].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f); - ((BoxMesh)_upperArmMeshes[1].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f); - ((BoxMesh)_forearmMeshes[0].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f); - ((BoxMesh)_forearmMeshes[1].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f); - - SetRigVisible(true); - GD.Print($"BodyRig: proportions calibrated from head height {headHeight:F2} m " + - $"(arm total {armTotal:F2} m, upper {_upperLen:F2} m, fore {_foreLen:F2} m, shoulder half-width {_shoulderHalfWidth:F2} m)"); - } - - private Vector3 SolveArm(int index, Vector3 shoulder, Vector3 side, RigidBody3D body, MeshInstance3D upperMesh, MeshInstance3D foreMesh, float delta, out bool decay) - { - Vector3 rawTarget = body != null - ? body.GlobalPosition + body.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset) - : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); - - float reach = _upperLen + _foreLen; - Vector3 shoulderToTarget = rawTarget - shoulder; - if (shoulderToTarget.LengthSquared() < 0.0001f) - shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f); - - float rawDist = shoulderToTarget.Length(); - float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor); - Vector3 dir = shoulderToTarget / rawDist; - Vector3 clampedTarget = shoulder + dir * d; - - Vector3 push = Vector3.Zero; - decay = false; - if (rawDist > reach) - { - push = (rawTarget - clampedTarget).Normalized(); - } - else if (rawDist < reach * ReachDecayFactor) - { - decay = true; - } - - float cosA = (_upperLen * _upperLen + d * d - _foreLen * _foreLen) / (2f * _upperLen * d); - float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f)); - - Vector3 handForward = body != null ? body.GlobalTransform.Basis * Vector3.Forward : Vector3.Zero; - Vector3 pole = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f; - pole -= dir * pole.Dot(dir); - pole -= handForward * PoleHandWeight; - Vector3 poleDir; - if (pole.LengthSquared() < 0.0001f) - poleDir = _lastPole[index] == Vector3.Zero ? Vector3.Down : _lastPole[index]; - else - { - poleDir = _lastPole[index].Lerp(pole, 1f - Mathf.Exp(-delta * PoleSmoothRate)); - if (poleDir.LengthSquared() < 0.0001f) - poleDir = _lastPole[index]; - poleDir = poleDir.Normalized(); - _lastPole[index] = poleDir; - } - - Vector3 axis = dir.Cross(poleDir); - if (axis.LengthSquared() < 0.0001f) - axis = dir.Cross(Vector3.Up); - if (axis.LengthSquared() < 0.0001f) - axis = Vector3.Right; - axis = axis.Normalized(); - - Vector3 elbowPos = shoulder + dir.Rotated(axis, angle) * _upperLen; - Vector3 foreEnd = elbowPos + (clampedTarget - elbowPos).Normalized() * _foreLen; - - PlaceSegment(upperMesh, shoulder, elbowPos); - PlaceSegment(foreMesh, elbowPos, foreEnd); - - return push; - } - + // Stretches the unit-length box between two joints so the drawn arm stays connected even + // when a physical segment is held back by contact. private static void PlaceSegment(MeshInstance3D mesh, Vector3 from, Vector3 to) { Vector3 segment = to - from; - if (segment.LengthSquared() < 0.000001f) + float length = segment.Length(); + if (length < 0.001f) return; - Vector3 y = segment.Normalized(); + Vector3 y = segment / length; Vector3 x = Vector3.Up.Cross(y); if (x.LengthSquared() < 0.0001f) x = Vector3.Right.Cross(y); x = x.Normalized(); Vector3 z = x.Cross(y); - mesh.GlobalTransform = new Transform3D(new Basis(x, y, z), (from + to) * 0.5f); + float scale = length / ((BoxMesh)mesh.Mesh).Size.Y; + mesh.GlobalTransform = new Transform3D(new Basis(x, y * scale, z), (from + to) * 0.5f); } } diff --git a/scripts/CollapseSpike.cs b/scripts/CollapseSpike.cs index 680b419..a102d33 100644 --- a/scripts/CollapseSpike.cs +++ b/scripts/CollapseSpike.cs @@ -1,7 +1,8 @@ using System.Collections.Generic; using Godot; -// Headless regression for the Stage-1 playtest collapse (2026-10-01). Two families: +// Headless regression for the Stage-1 playtest collapse (2026-10-01), now with the full Stage-2 +// body (chest + PhysicsArms + hands). 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 @@ -39,6 +40,7 @@ public partial class CollapseSpike : Node3D private Node3D _world; private Node3D _head; private PhysicsTorso _torso; + private PhysicsArms _arms; private readonly RigidBody3D[] _hands = new RigidBody3D[2]; private readonly Node3D[] _targets = new Node3D[2]; private readonly ConeTwistJoint3D[] _welds = new ConeTwistJoint3D[2]; @@ -189,6 +191,9 @@ public partial class CollapseSpike : Node3D _world.AddChild(_targets[h]); _targetBase[h] = handPos; } + + _arms = new PhysicsArms { HeadSource = _head, Torso = _torso, HandLeft = _hands[0], HandRight = _hands[1] }; + _world.AddChild(_arms); } public override void _PhysicsProcess(double delta) @@ -258,6 +263,15 @@ public partial class CollapseSpike : Node3D Check("handR", _hands[1]); if (_anvil != null) Check("anvil", _anvil); + for (int side = 0; side < 2; side++) + { + if (_arms.UpperArm(side) == null) + continue; + Check($"upper arm {side}", _arms.UpperArm(side)); + Check($"forearm {side}", _arms.Forearm(side)); + if (_arms.Forearm(side).LinearVelocity.Length() > RunawaySpeed || _arms.UpperArm(side).LinearVelocity.Length() > RunawaySpeed) + Fail($"arm {side} runaway"); + } foreach (Node n in GetTree().GetNodesInGroup("prop")) { if (n is not RigidBody3D prop || prop.IsQueuedForDeletion()) diff --git a/scripts/PhysicsArms.cs b/scripts/PhysicsArms.cs new file mode 100644 index 0000000..7b82ef7 --- /dev/null +++ b/scripts/PhysicsArms.cs @@ -0,0 +1,258 @@ +using Godot; + +// Body-physics Stage 2: physical arm segments. Upper arm + forearm per side are chase-driven +// RigidBody3Ds (no joints — Box3D has no usable 3-DOF shoulder/wrist joint, and AGENTS.md +// forbids joints between tracked bodies). Each segment PD-chases its analytic 2-bone IK pose, +// solved every physics tick from the PHYSICAL chest shoulders to the PHYSICAL hand bodies. +// Hierarchical budgets (Nimso's muscle model): hand 90 N < forearm < upper arm < chest 2400 N. +// The arm IK + reach budget (torso lean on overreach) live here; BodyRig only draws. +public partial class PhysicsArms : Node +{ + private const float BodyMassStat = 60f; + private const float ForearmMass = 1.2f; + private const float UpperArmMass = 2.0f; + private const float ForearmForcePerKg = 3f; // 180 N at 60 kg + private const float UpperArmForcePerKg = 6f; // 360 N at 60 kg + private const float SegmentMaxSpin = 40f; + private const float ForearmMaxAlpha = 1500f; + private const float UpperArmMaxAlpha = 1000f; + private const float SegmentInset = 0.04f; + public const float SnapDistance = 1f; + + private const float ShoulderRise = 0.25f; + private const float ReachBudgetMax = 0.25f; + private const float ReachVerticalMin = -0.05f; + private const float ReachVerticalMax = 0.12f; + private const float ReachPushSpeed = 0.8f; + private const float ReachDecaySpeed = 0.4f; + private const float ReachDecayFactor = 0.92f; + private const float WristOffset = 0.06f; + private const float SoftIKFactor = 0.98f; + private const float PoleHandWeight = 0.25f; + private const float PoleSmoothRate = 10f; + + public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg; + public static float UpperArmMaxForce => BodyMassStat * UpperArmForcePerKg; + + // Injected by headless spikes; resolved from the main scene otherwise. + public Node3D HeadSource; + public PhysicsTorso Torso; + public RigidBody3D HandLeft; + public RigidBody3D HandRight; + + public bool Calibrated { get; private set; } + public float UpperLen { get; private set; } + public float ForeLen { get; private set; } + public float ShoulderHalfWidth { get; private set; } + + private readonly RigidBody3D[] _upper = new RigidBody3D[2]; + private readonly RigidBody3D[] _fore = new RigidBody3D[2]; + private readonly Vector3[] _shoulder = new Vector3[2]; + private readonly Vector3[] _elbowTarget = new Vector3[2]; + private readonly Vector3[] _wristTarget = new Vector3[2]; + private readonly Vector3[] _lastPole = new Vector3[2]; + private Vector3 _torsoOffset; + private Basis _torsoBasis = Basis.Identity; + + public RigidBody3D UpperArm(int side) => _upper[side]; + public RigidBody3D Forearm(int side) => _fore[side]; + public Vector3 Shoulder(int side) => _shoulder[side]; + public Vector3 ElbowTarget(int side) => _elbowTarget[side]; + public Vector3 WristTarget(int side) => _wristTarget[side]; + + // Physical elbow: where the two segment bodies meet (mean of their joint-side ends). + public Vector3 Elbow(int side) => + (SegmentEnd(_upper[side], UpperLen, 1f) + SegmentEnd(_fore[side], ForeLen, -1f)) * 0.5f; + + public Vector3 Wrist(int side) => SegmentEnd(_fore[side], ForeLen, 1f); + + public override void _Ready() + { + if (HeadSource == null) + { + HeadSource = GetNode("../XROrigin3D/XRCamera3D"); + Torso = GetNode("../PhysicsTorso"); + HandLeft = PhysicsHands.GetBody(GetNode("../XROrigin3D/LeftHand")); + HandRight = PhysicsHands.GetBody(GetNode("../XROrigin3D/RightHand")); + } + } + + public override void _PhysicsProcess(double delta) + { + float dt = (float)delta; + if (!Calibrated) + { + float headHeight = HeadSource.GlobalPosition.Y; + if (headHeight <= 0.5f || Torso?.Body == null) + return; + Calibrate(headHeight); + } + + _torsoBasis = Torso.Body.GlobalTransform.Basis; + Vector3 topCenter = Torso.Body.GlobalPosition + Vector3.Up * ShoulderRise; + _shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth; + _shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth; + + Vector3 push = SolveArm(0, -_torsoBasis.X, HandLeft, dt, out bool decayLeft); + push += SolveArm(1, _torsoBasis.X, HandRight, dt, out bool decayRight); + UpdateReachBudget(push, decayLeft || decayRight, dt); + + for (int side = 0; side < 2; side++) + { + Drive(_upper[side], SegmentTarget(_shoulder[side], _elbowTarget[side], _upper[side]), UpperArmMaxForce, UpperArmMaxAlpha, dt); + Drive(_fore[side], SegmentTarget(_elbowTarget[side], _wristTarget[side], _fore[side]), ForearmMaxForce, ForearmMaxAlpha, dt); + } + } + + private void Calibrate(float headHeight) + { + Calibrated = true; + float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f); + UpperLen = 0.5f * armTotal; + ForeLen = 0.5f * armTotal; + ShoulderHalfWidth = 0.16f + headHeight * 0.02f; + + for (int side = 0; side < 2; side++) + { + string name = side == 0 ? "Left" : "Right"; + _upper[side] = MakeSegment($"{name}UpperArmBody", UpperArmMass, new Vector3(0.07f, UpperLen - SegmentInset, 0.07f)); + _fore[side] = MakeSegment($"{name}ForearmBody", ForearmMass, new Vector3(0.06f, ForeLen - SegmentInset, 0.06f)); + } + GD.Print($"PhysicsArms: proportions calibrated from head height {headHeight:F2} m " + + $"(arm total {armTotal:F2} m, upper {UpperLen:F2} m, fore {ForeLen:F2} m, shoulder half-width {ShoulderHalfWidth:F2} m); " + + $"budgets fore {ForearmMaxForce:F0} N, upper {UpperArmMaxForce:F0} N"); + } + + private RigidBody3D MakeSegment(string name, float mass, Vector3 size) + { + RigidBody3D body = new() + { + Name = name, + Mass = mass, + GravityScale = 0f, + ContinuousCd = true, + CollisionLayer = 0b10000, // layer 5 = player arms + CollisionMask = 0b1, // world + props only; own chest/hands/arms excluded + }; + body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } }); + AddChild(body); + // Spawn somewhere finite; the first Drive snaps it onto its target (> SnapDistance). + body.GlobalPosition = Torso.Body.GlobalPosition + Vector3.Down * 100f; + return body; + } + + private static void Drive(RigidBody3D body, Transform3D target, float maxForce, float maxAlpha, float dt) + { + if (BodyMath.ResetIfNonFinite(body, target, "PhysicsArms")) + return; + if (body.GlobalPosition.DistanceTo(target.Origin) > SnapDistance) + { + body.GlobalTransform = target; + body.LinearVelocity = Vector3.Zero; + body.AngularVelocity = Vector3.Zero; + return; + } + + Vector3 desiredVelocity = (target.Origin - body.GlobalPosition) / dt; + Vector3 force = ((desiredVelocity - body.LinearVelocity) / dt * body.Mass).LimitLength(maxForce); + body.LinearVelocity += force / body.Mass * dt; + + Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), + body.GlobalTransform.Basis.GetRotationQuaternion()); + Vector3 spinDesired = (rotErr / dt).LimitLength(SegmentMaxSpin); + body.AngularVelocity += (spinDesired - body.AngularVelocity).LimitLength(maxAlpha * dt); + } + + // Segment pose from joint A to joint B: local +Y runs A→B; twist follows the body's current + // X axis so the long-axis roll never flips (a box arm has no meaningful roll). + private static Transform3D SegmentTarget(Vector3 a, Vector3 b, RigidBody3D body) + { + Vector3 y = b - a; + if (y.LengthSquared() < 1e-8f) + y = Vector3.Down; + y = y.Normalized(); + Vector3 x = body.GlobalTransform.Basis.X; + x -= y * x.Dot(y); + if (x.LengthSquared() < 1e-6f) + { + x = y.Cross(Vector3.Forward); + if (x.LengthSquared() < 1e-6f) + x = y.Cross(Vector3.Right); + } + x = x.Normalized(); + return new Transform3D(new Basis(x, y, x.Cross(y)), (a + b) * 0.5f); + } + + private static Vector3 SegmentEnd(RigidBody3D body, float length, float sign) => + body.GlobalPosition + body.GlobalTransform.Basis.Y * (sign * length * 0.5f); + + private void UpdateReachBudget(Vector3 push, bool decay, float dt) + { + if (push != Vector3.Zero) + _torsoOffset += push * (dt * ReachPushSpeed); + else if (decay) + _torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt); + + Vector3 horizontal = new Vector3(_torsoOffset.X, 0f, _torsoOffset.Z).LimitLength(ReachBudgetMax); + _torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z); + Torso.ReachOffset = _torsoOffset; + } + + // Analytic 2-bone IK (moved from BodyRig): soft-IK clamp, torso-frame elbow pole with the + // shoulder→hand axis projected out, hand-forward push, exp-smoothed pole. + private Vector3 SolveArm(int index, Vector3 side, RigidBody3D hand, float delta, out bool decay) + { + Vector3 shoulder = _shoulder[index]; + Vector3 rawTarget = hand != null + ? hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset) + : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); + + float reach = UpperLen + ForeLen; + Vector3 shoulderToTarget = rawTarget - shoulder; + if (shoulderToTarget.LengthSquared() < 0.0001f) + shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f); + + float rawDist = shoulderToTarget.Length(); + float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor); + Vector3 dir = shoulderToTarget / rawDist; + Vector3 clampedTarget = shoulder + dir * d; + + Vector3 push = Vector3.Zero; + decay = false; + if (rawDist > reach) + push = (rawTarget - clampedTarget).Normalized(); + else if (rawDist < reach * ReachDecayFactor) + decay = true; + + float cosA = (UpperLen * UpperLen + d * d - ForeLen * ForeLen) / (2f * UpperLen * d); + float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f)); + + Vector3 handForward = hand != null ? hand.GlobalTransform.Basis * Vector3.Forward : Vector3.Zero; + Vector3 pole = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f; + pole -= dir * pole.Dot(dir); + pole -= handForward * PoleHandWeight; + Vector3 poleDir; + if (pole.LengthSquared() < 0.0001f) + poleDir = _lastPole[index] == Vector3.Zero ? Vector3.Down : _lastPole[index]; + else + { + poleDir = _lastPole[index].Lerp(pole, 1f - Mathf.Exp(-delta * PoleSmoothRate)); + if (poleDir.LengthSquared() < 0.0001f) + poleDir = _lastPole[index]; + poleDir = poleDir.Normalized(); + _lastPole[index] = poleDir; + } + + Vector3 axis = dir.Cross(poleDir); + if (axis.LengthSquared() < 0.0001f) + axis = dir.Cross(Vector3.Up); + if (axis.LengthSquared() < 0.0001f) + axis = Vector3.Right; + axis = axis.Normalized(); + + Vector3 elbow = shoulder + dir.Rotated(axis, angle) * UpperLen; + _elbowTarget[index] = elbow; + _wristTarget[index] = elbow + (clampedTarget - elbow).Normalized() * ForeLen; + return push; + } +} diff --git a/scripts/PhysicsArms.cs.uid b/scripts/PhysicsArms.cs.uid new file mode 100644 index 0000000..46a48b9 --- /dev/null +++ b/scripts/PhysicsArms.cs.uid @@ -0,0 +1 @@ +uid://boeceet3aku24 diff --git a/scripts/PropShop.cs b/scripts/PropShop.cs index dc5ea5a..135d8dc 100644 --- a/scripts/PropShop.cs +++ b/scripts/PropShop.cs @@ -45,7 +45,7 @@ public partial class PropShop : Node3D { Color wood = new(0.45f, 0.3f, 0.18f); StaticBody3D table = new(); - table.CollisionMask = 0b1101; // world + player body + hands + table.CollisionMask = 0b11101; // world + player body + hands + arms table.AddChild(new CollisionShape3D { Position = new Vector3(0, 0.725f, -0.6f), @@ -96,7 +96,7 @@ public partial class PropShop : Node3D ContinuousCd = spec.Mass < 0.1f, ContactMonitor = true, MaxContactsReported = 4, - CollisionMask = 0b1101, // world + player body + hands + CollisionMask = 0b11101, // world + player body + hands + arms }; body.AddToGroup("prop");