diff --git a/AGENTS.md b/AGENTS.md index dd40eea..a4e40b6 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -215,7 +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, 2 = UI (panel mask 0 — props pass through even when shown); layers live on + **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 + 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); grab = palm-proximity only, per-hand palm-face offset (left +X, right −X — taste). @@ -225,7 +228,20 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x (arm IK, IK stability/deep, grabbing, two-handed grab/Box3D internals, addon weld patch, body physics hugs, body physics Godot facts) — READ before re-researching or changing grab/IK/body. Stink-O's box3d-godot (custom components) = documented plan-B. -7. **Getting a body (rig) — placeholder v1.2 (cube character, agent-built).** `BodyRig.cs`: +8. **Body physics Stage 1 — PHYSICAL CHEST, implemented 2026-10-01.** `PhysicsTorso.cs`: + one chase-driven RigidBody3D (30 kg box, gravity off, CCD) toward an HMD-derived target — + the PhysicsHands PD pattern at body mass (2400 N budget), orientation = slewed + 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). + `BodyRig.cs` is VISUALS-only now (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). Stage 2 (physical + arm chains + hierarchical per-joint budgets, Nimso/Boneworks tier) awaits the owner's + Stage-1 feel verdict; 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, 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 @@ -236,7 +252,7 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x Reach budget hysteresis unchanged. Full sourced findings in `research/vr-arm-ik.md` and `research/vr-arm-ik-deep.md` (HL:Alyx went hands-only on purpose; SkeletonIK3D deprecated → custom SkeletonModifier3D when a real skeleton arrives; LookAtModifier3D for head→neck). - Physical torso/head capsules later. EMBODYING (view-from-character-perspective) comes after + EMBODYING (view-from-character-perspective) comes after 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. diff --git a/HANDOFF.md b/HANDOFF.md index aff93f1..cab3b16 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -65,7 +65,8 @@ TERMINOLOGY (owner-corrected — use precisely): near (`WorldPanel.IsBlocking`). **All deltas and turn pivots in WORLD space** (tracking-space math made turns orbit the map center — do not regress). Drives `XROrigin3D` directly. - **PhysicsHands.cs** — physical palm bodies chasing the controller poses: 0.4 kg boxes (flat slab, - thumb-side face), gravity off, CCD, layer 1/mask 1. Chase = desired velocity `(target-pos)/dt` + thumb-side face), gravity off, CCD, **layer 4 (player hands)/mask 1|4** — NOT colliding with the + chest (layer 3), excluded both ways. Chase = desired velocity `(target-pos)/dt` force-clamped to `HandMaxForce` 90 N; rotation hard-locked by setting angular velocity to the controller's rotation error (clamped to 120 rad/s). Safety: stuck (> 0.2 m for 1 s) → snap + log; > `SnapDistance` 2 m → silent teleport snap — BOTH SUSPENDED for a hand while @@ -119,22 +120,26 @@ 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. -- **BodyRig.cs** — placeholder cube "getting a body" v1.1 (all primitives boxes per owner). Head - 0.15³ pushed 6 cm behind the eyes (`HeadBackOffset`); torso 0.22×0.5×0.13, dropped 0.42 and - retracted 6 cm; yaw basis from HMD (flattened forward). Proportions calibrated at runtime from - head height (once, after height > 0.5 m): armTotal = 0.46×headHeight clamped [0.5, 0.72], - 50/50 upper/fore split, shoulder half-width 0.16 + 0.02×height. Arms: wrist-anchored IK target - (palm base = hand body +Z·`WristOffset` 0.06, not palm center) with SOFT IK — shoulder→hand - distance clamped to reach×`SoftIKFactor` (0.98) BEFORE the law-of-cosines solve, so the arm is - never perfectly straight (if near-extension shake persists, lower `SoftIKFactor` toward 0.95). - Elbow pole = down + back + slight outward, projected OUT of the shoulder→hand axis, minus hand - forward × `PoleHandWeight` (0.25), exp-smoothed at `PoleSmoothRate` (10/s). Torso yaw SPRING, - decoupled from the head: HMD yaw base blended toward hands-derived yaw by forwardness × - `TorsoYawHandsWeight` (forwardness = horizontal dot / `TorsoYawForwardness` 0.3 m, clamped 0–1), - exp smoothing `TorsoYawSmoothRate` (12/s ≈ 200 ms response). Torso reach budget WITH HYSTERESIS: - push only past full reach (0.8 m/s), decay - only below 92% of reach (0.4 m/s, `MoveToward` zero); offset clamped horizontal ≤ 0.25 m, - vertical [−0.05, +0.12]. Hidden until calibrated. +- **PhysicsTorso.cs** — the player's PHYSICAL CHEST (body-physics Stage 1, implemented 2026-10-01). + 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 + 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 + covers teleports. Camera stays KINEMATIC — the torso chases the head, never the reverse + (Boneworks sickness lesson). Yaw spring (HMD + hands blend) MOVED here from BodyRig. + Collision: layer 3 (player body), mask 1|3 — props/floor touch it, own hands do NOT + (hands moved to layer 4; excluded both ways — no self-collision, VRChat/Nimso precedent). + Owns its torso mesh (hidden until the body first arrives at target). Spike: + `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. - **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 @@ -148,6 +153,12 @@ TERMINOLOGY (owner-corrected — use precisely): bodies welded via ConeTwistJoint3D (bias 0 rigid, then bias 0.5 soft); asserts relative-pose rigidity under torque/force (rot < 2°, sep < 1 cm), bounded excursion + full recovery for the soft weld. Run this FIRST whenever the addon binaries change. +- **BodySpike.cs** + `scenes/body_spike.tscn` — headless regression for the physical torso: + pair-rule proofs on Box3D (one-way floor contact; hand-config body passes through the chest + with the chest undisturbed; prop-config body transfers contact), then track (walk pattern, + error < 6 cm), yaw (90°/1 s turn: peak < 15°, settled < 4°), impact (5 kg at 3 m/s: dip + < 0.2 m, recovers < 2 cm, never tunnels, rebounds), teleport snap, self-right (< 3° after + a pitch impulse). Must print **"BODY: overall PASS"** 3× deterministic. Research (`research/`): seven full sourced reports — `vr-arm-ik.md`, `vr-arm-ik-deep.md`, `vr-grabbing.md`, `vr-two-handed-grab.md` (grab v5.0→5.2 history + the verified Box3D pin @@ -163,7 +174,8 @@ 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, BodyRig, FadeLayer/Fade. Action map: `openxr_action_map.tres`. +PropShop, PropGrabber, PhysicsHands, PhysicsTorso, BodyRig, FadeLayer/Fade. Action map: `openxr_action_map.tres`. +Floor StaticBody scans layer 3 (collision_mask 13). ## 4. Architecture decisions that must not regress @@ -188,7 +200,10 @@ PropShop, PropGrabber, PhysicsHands, BodyRig, FadeLayer/Fade. Action map: `openx 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); hands layer 1. Layers/masks live on `CollisionObject3D`, never `CollisionShape3D`. + 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 + body_spike proves the pair rules on Box3D. - **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); @@ -261,7 +276,10 @@ PropShop, PropGrabber, PhysicsHands, BodyRig, FadeLayer/Fade. Action map: `openx | 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 | -| `TorsoYawHandsWeight` / `TorsoYawForwardness` / `TorsoYawSmoothRate` | BodyRig.cs | 0.6 / 0.3 m / 12 | torso yaw spring: hands blend / forwardness normalizer / exp smoothing (≈200 ms) | +| `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) | @@ -277,21 +295,17 @@ PropShop, PropGrabber, PhysicsHands, BodyRig, FadeLayer/Fade. Action map: `openx ## 7. Next steps (in order) -1. **Full physics body track — DECISION PENDING (owner holds implementation)** — the owner - wants to pursue a full physics VR body (also as reusable tech for a second project idea); - the hug should be mostly physics with detection for future interactivity. Research done: - `research/vr-body-physics-hugs.md` (prior art + phased design; §6 = Nimso's "Hybrid - Presence", the closest known implementation; §7 = owner-extracted video breakdowns of - Project TX: hierarchical per-joint force budgets, script-relaxation hand adaptation, - active-ragdoll NPCs with grab anchors) and `research/vr-body-physics-godot.md` (verified - stack facts: no Generic6DOF, hinge velocity motors work, PD-follow proven by the hands). - Research verdict: the technique is an independent-implementation match of our existing - chase-drive architecture; no more video extraction needed now (conditional: Spideriini - elbow handling if Stage-2 elbow jitter bites; Test Chamber if a locomotion stage ever - starts). Recommendation shape: staged torso → 2-segment arm chains (chase-driven, - Boneworks/Nimso tier, NOT per-bone skeleton sim), test hugging props; muscle model = - hierarchical budgets (hand ≺ forearm ≺ shoulder ≺ torso) scaled from body mass. - Character model: keep boxes for the physics work (owner asked; see research §5-6). +1. **Full physics body track — Stage 1 (chest body) IMPLEMENTED, validate in-headset** — + `PhysicsTorso.cs` is live: chase chest (30 kg, 2400 N), yaw spring moved in, collision + layers reworked (3 = body, 4 = hands, no self-collision). Owner playtests: chest contact + with props (lean a crate on it, catch the bowling ball against it), shove response + (props push the chest, it recovers — camera never moves), turn following (torso lags + head naturally), weight feel. Hug testing on props as agreed. Headless: `body_spike` + PASS 3×; all other spikes still PASS 3×. **Stage 2 (arm chains, hierarchical budgets, + Nimso/Boneworks tier) awaits Stage-1 feel verdict** — see + `research/vr-body-physics-hugs.md` §5/§7 + `research/vr-body-physics-godot.md`. + Tuning knobs live in HANDOFF §6 (PhysicsTorso rows); first levers if feel is off: + `MaxForce` (gives more/less), `OrientKp` (torso yaw stiffness). 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/scenes/body_spike.tscn b/scenes/body_spike.tscn new file mode 100644 index 0000000..a0ccd97 --- /dev/null +++ b/scenes/body_spike.tscn @@ -0,0 +1,6 @@ +[gd_scene load_steps=2 format=3] + +[ext_resource type="Script" path="res://scripts/BodySpike.cs" id="1_spike"] + +[node name="BodySpike" type="Node3D"] +script = ExtResource("1_spike") diff --git a/scenes/main.tscn b/scenes/main.tscn index 4553e59..e9b753c 100644 --- a/scenes/main.tscn +++ b/scenes/main.tscn @@ -1,4 +1,4 @@ -[gd_scene load_steps=19 format=3] +[gd_scene load_steps=20 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"] @@ -9,6 +9,7 @@ [ext_resource type="Script" path="res://scripts/PropGrabber.cs" id="7_propgrabber"] [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"] [sub_resource type="ProceduralSkyMaterial" id="ProceduralSkyMaterial_sky"] sky_horizon_color = Color(0.66, 0.72, 0.81, 1) @@ -55,6 +56,7 @@ shadow_bias = 0.04 shadow_normal_bias = 0.5 [node name="Floor" type="StaticBody3D" parent="."] +collision_mask = 13 [node name="FloorShape" type="CollisionShape3D" parent="Floor"] transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, -0.05, 0) @@ -113,6 +115,9 @@ script = ExtResource("7_propgrabber") [node name="PhysicsHands" type="Node" parent="."] script = ExtResource("8_physicshands") +[node name="PhysicsTorso" type="Node" parent="."] +script = ExtResource("10_physicsTorso") + [node name="BodyRig" type="Node3D" parent="."] script = ExtResource("9_bodyrig") diff --git a/scripts/BodyRig.cs b/scripts/BodyRig.cs index cd3d78c..bdee442 100644 --- a/scripts/BodyRig.cs +++ b/scripts/BodyRig.cs @@ -4,8 +4,6 @@ public partial class BodyRig : Node3D { private const float HeadBackOffset = 0.06f; private const float HeadSize = 0.15f; - private const float TorsoDrop = 0.42f; - private const float TorsoBackOffset = 0.06f; private const float ShoulderRise = 0.25f; private const float ReachBudgetMax = 0.25f; private const float ReachVerticalMin = -0.05f; @@ -17,16 +15,13 @@ public partial class BodyRig : Node3D private const float SoftIKFactor = 0.98f; private const float PoleHandWeight = 0.25f; private const float PoleSmoothRate = 10f; - private const float TorsoYawHandsWeight = 0.6f; - private const float TorsoYawForwardness = 0.3f; - private const float TorsoYawSmoothRate = 12f; private XRCamera3D _camera; private XRController3D _left; private XRController3D _right; + private PhysicsTorso _torso; private MeshInstance3D _headMesh; - private MeshInstance3D _torsoMesh; private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2]; private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2]; @@ -37,7 +32,6 @@ public partial class BodyRig : Node3D private Vector3 _torsoOffset = Vector3.Zero; private Basis _yawBasis = Basis.Identity; private Basis _torsoBasis = Basis.Identity; - private float _torsoYaw; private readonly Vector3[] _lastPole = new Vector3[2]; public override void _Ready() @@ -45,9 +39,9 @@ public partial class BodyRig : Node3D _camera = GetNode("../XROrigin3D/XRCamera3D"); _left = GetNode("../XROrigin3D/LeftHand"); _right = GetNode("../XROrigin3D/RightHand"); + _torso = GetNode("../PhysicsTorso"); _headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f)); - _torsoMesh = MakeMesh(new BoxMesh { Size = new Vector3(0.22f, 0.5f, 0.13f) }, new Color(0.4f, 0.45f, 0.55f)); _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)); @@ -67,7 +61,6 @@ public partial class BodyRig : Node3D private void SetRigVisible(bool visible) { _headMesh.Visible = visible; - _torsoMesh.Visible = visible; foreach (MeshInstance3D m in _upperArmMeshes) m.Visible = visible; foreach (MeshInstance3D m in _forearmMeshes) @@ -86,15 +79,14 @@ public partial class BodyRig : Node3D return; Calibrate(headHeight); } + if (_torso.Body == null) + return; Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset; _headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos); - UpdateTorsoYaw(headPos, dt); - - Vector3 torsoBack = _torsoBasis * Vector3.Back; - Vector3 torsoCenter = headPos + Vector3.Down * TorsoDrop + torsoBack * TorsoBackOffset + _torsoOffset; - _torsoMesh.GlobalTransform = new Transform3D(_torsoBasis, torsoCenter); + _torsoBasis = _torso.Body.GlobalTransform.Basis; + Vector3 torsoCenter = _torso.Body.GlobalPosition; Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise; Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth; @@ -115,6 +107,7 @@ public partial class BodyRig : Node3D 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 UpdateYawBasis() @@ -126,37 +119,9 @@ public partial class BodyRig : Node3D _yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up); } - private void UpdateTorsoYaw(Vector3 headPos, float delta) - { - float hmdYaw = YawOf(-_yawBasis.Z); - RigidBody3D leftBody = PhysicsHands.GetBody(_left); - RigidBody3D rightBody = PhysicsHands.GetBody(_right); - float targetYaw = hmdYaw; - if (leftBody != null && rightBody != null) - { - Vector3 toHands = (leftBody.GlobalPosition + rightBody.GlobalPosition) * 0.5f - headPos; - Vector3 horizontal = new(toHands.X, 0f, toHands.Z); - if (horizontal.LengthSquared() > 0.0001f) - { - float handsYaw = YawOf(horizontal.Normalized()); - float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / TorsoYawForwardness, 0f, 1f); - targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * TorsoYawHandsWeight); - } - } - _torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * TorsoYawSmoothRate)); - Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw)); - _torsoBasis = Basis.LookingAt(fwd, Vector3.Up); - } - - private static float YawOf(Vector3 forward) - { - return Mathf.Atan2(-forward.X, -forward.Z); - } - private void Calibrate(float headHeight) { _calibrated = true; - _torsoYaw = YawOf(-_yawBasis.Z); float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f); _upperLen = 0.5f * armTotal; _foreLen = 0.5f * armTotal; diff --git a/scripts/BodySpike.cs b/scripts/BodySpike.cs new file mode 100644 index 0000000..475e8b8 --- /dev/null +++ b/scripts/BodySpike.cs @@ -0,0 +1,328 @@ +using System.Collections.Generic; +using Godot; + +public partial class BodySpike : Node3D +{ + private enum Phase { Settle, Pairs, Track, Yaw, Impact, Snap, Upright } + + private const float PhaseSettle = 0.8f; + private const float PhasePairs1 = 1.3f; + private const float PhasePairs2 = 1.7f; + private const float PhaseTrack = 3.5f; + private const float PhaseYaw = 2f; + private const float PhaseImpact = 2f; + private const float PhaseSnap = 0.3f; + private const float PhaseUpright = 2f; + + private const float TrackPosLimit = 0.06f; + private const float TrackSpeedLimit = 8f; + private const float YawPeakLimitDeg = 15f; + private const float YawSettleLimitDeg = 4f; + private const float ImpactDipLimit = 0.2f; + private const float RecoverLimit = 0.02f; + private const float ImpactorMinDistLimit = 0.1f; + private const float ImpactorSpeedAfterHitLimit = 1.5f; + private const float SnapLimit = 0.05f; + private const float UprightLimitDeg = 3f; + private const float YawHoldLimitDeg = 5f; + + private PhysicsTorso _torso; + private Node3D _head; + private RigidBody3D _ball; + private RigidBody3D _handProbe; + private RigidBody3D _prop; + private RigidBody3D _impactor; + + private Phase _phase = Phase.Settle; + private float _phaseStart; + private float _elapsed; + private readonly List _resultNames = new(); + private readonly List _resultOk = new(); + + private Vector3 _chestRef; + private bool _chestStillDuringHandDrop; + private float _chestDropDip; + private float _maxTrackErr; + private float _maxTrackSpeed; + private float _maxYawErrAfterRamp; + private float _yawErrEnd; + private float _maxImpactDip; + private float _minImpactorDist = float.MaxValue; + private float _impactorTouchTime = -1f; + private float _impactorSpeedAfterHit = -1f; + + public override void _Ready() + { + BuildWorld(); + _head = new Node3D { Position = new Vector3(0f, 1.6f, 0f) }; + AddChild(_head); + + _torso = new PhysicsTorso { HeadSource = _head }; + AddChild(_torso); + GD.Print("BODY: body spike scene ready"); + } + + private void BuildWorld() + { + StaticBody3D ground = new() { CollisionLayer = 1, CollisionMask = 0 }; + ground.AddChild(new CollisionShape3D + { + Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) }, + Position = new Vector3(0f, -0.05f, 0f), + }); + AddChild(ground); + + _ball = new RigidBody3D + { + Mass = 1f, + Position = new Vector3(-2f, 0.4f, 0f), + CollisionLayer = 1, + CollisionMask = 1, + PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.8f }, + }; + _ball.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.05f } }); + AddChild(_ball); + + _handProbe = new RigidBody3D + { + Mass = 0.4f, + Position = new Vector3(0f, 1.7f, 0.12f), + CollisionLayer = 0b1000, + CollisionMask = 0b1001, + PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.8f }, + }; + _handProbe.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.035f, 0.09f, 0.12f) } }); + AddChild(_handProbe); + + _prop = new RigidBody3D + { + Mass = 2f, + Position = new Vector3(0f, 1.9f, 0.12f), + CollisionLayer = 1, + CollisionMask = 0b1101, + PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.3f, Friction = 0.8f }, + }; + _prop.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } }); + AddChild(_prop); + _prop.Freeze = true; + _prop.Visible = false; + + _impactor = new RigidBody3D + { + Mass = 5f, + GravityScale = 0f, + CollisionLayer = 1, + CollisionMask = 0b1101, + PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.6f }, + }; + _impactor.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } }); + AddChild(_impactor); + _impactor.Freeze = true; + } + + public override void _PhysicsProcess(double delta) + { + float dt = (float)delta; + _elapsed += dt; + float phaseT = _elapsed - _phaseStart; + + Vector3 p = _torso.Body.GlobalPosition; + if (float.IsNaN(p.X) || float.IsNaN(p.Y) || float.IsNaN(p.Z)) + { + Record("no NaN in body state", false); + Finish(); + return; + } + + switch (_phase) + { + case Phase.Settle: + if (phaseT >= PhaseSettle) + { + _chestRef = _torso.Body.GlobalPosition; + StartPhase(Phase.Pairs); + GD.Print("BODY: pairs phase — one-way floor, hand-through-chest exclusion, prop contact"); + } + break; + + case Phase.Pairs: + { + if (phaseT >= PhasePairs1 && _prop.Freeze) + { + _chestStillDuringHandDrop = _torso.Body.GlobalPosition.DistanceTo(_chestRef) < 0.02f; + _chestRef = _torso.Body.GlobalPosition; + _chestDropDip = 0f; + _prop.Freeze = false; + _prop.Visible = true; + GD.Print("BODY: dropping 2 kg ball onto chest top"); + } + _chestDropDip = Mathf.Max(_chestDropDip, _torso.Body.GlobalPosition.DistanceTo(_chestRef)); + if (phaseT >= PhasePairs1 + PhasePairs2) + FinishPairs(); + break; + } + + case Phase.Track: + { + float t = phaseT; + Vector3 headPos = new(0.6f * Mathf.Sin(1.1f * t), 1.6f, 0.8f * Mathf.Sin(0.7f * t)); + Basis headBasis = new(new Quaternion(Vector3.Up, 0.6f * Mathf.Sin(0.5f * t))); + _head.GlobalTransform = new Transform3D(headBasis, headPos); + if (phaseT > 0.5f) + { + _maxTrackErr = Mathf.Max(_maxTrackErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition)); + _maxTrackSpeed = Mathf.Max(_maxTrackSpeed, _torso.Body.LinearVelocity.Length()); + } + if (phaseT >= PhaseTrack) + { + Record($"track error max {_maxTrackErr:F3} m (limit {TrackPosLimit})", _maxTrackErr < TrackPosLimit); + Record($"track speed max {_maxTrackSpeed:F2} m/s (limit {TrackSpeedLimit})", _maxTrackSpeed < TrackSpeedLimit); + StartPhase(Phase.Yaw); + GD.Print("BODY: yaw phase — head turns 90 deg over 1 s"); + } + break; + } + + case Phase.Yaw: + { + float yaw = Mathf.Pi / 2f * Mathf.Min(phaseT / 1f, 1f); + _head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), _head.GlobalPosition); + float err = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis)); + if (phaseT > 0.4f) + _maxYawErrAfterRamp = Mathf.Max(_maxYawErrAfterRamp, err); + if (phaseT > PhaseYaw - 0.5f) + _yawErrEnd = Mathf.Max(_yawErrEnd, err); + if (phaseT >= PhaseYaw) + { + Record($"yaw error peak {_maxYawErrAfterRamp:F1} deg (limit {YawPeakLimitDeg})", _maxYawErrAfterRamp < YawPeakLimitDeg); + Record($"yaw settled {_yawErrEnd:F1} deg (limit {YawSettleLimitDeg})", _yawErrEnd < YawSettleLimitDeg); + StartPhase(Phase.Impact); + GD.Print("BODY: impact phase — 5 kg ball at 3 m/s into the chest"); + } + break; + } + + case Phase.Impact: + { + _maxImpactDip = Mathf.Max(_maxImpactDip, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition)); + _minImpactorDist = Mathf.Min(_minImpactorDist, _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition)); + if (_impactorTouchTime >= 0f && _impactorSpeedAfterHit < 0f && phaseT > _impactorTouchTime + 0.2f) + _impactorSpeedAfterHit = _impactor.LinearVelocity.Length(); + if (phaseT >= PhaseImpact) + FinishImpact(); + break; + } + + case Phase.Snap: + { + if (phaseT >= PhaseSnap) + { + float err = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition); + Record($"snap re-locate (err {err:F3} m, limit {SnapLimit})", err < SnapLimit); + StartPhase(Phase.Upright); + _torso.Body.AngularVelocity += _torso.Body.GlobalTransform.Basis.X * 6f; + GD.Print("BODY: upright phase — pitch impulse, body must self-right"); + } + break; + } + + case Phase.Upright: + { + if (phaseT >= PhaseUpright) + FinishUpright(); + break; + } + } + + if (_phase == Phase.Impact && _impactorTouchTime < 0f + && _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition) < 0.25f) + _impactorTouchTime = _elapsed - _phaseStart; + } + + private void StartPhase(Phase phase) + { + _phase = phase; + _phaseStart = _elapsed; + if (phase == Phase.Impact) + { + _maxImpactDip = 0f; + _minImpactorDist = float.MaxValue; + _impactorTouchTime = -1f; + _impactorSpeedAfterHit = -1f; + _impactor.Freeze = false; + Vector3 chest = _torso.Body.GlobalPosition; + Vector3 fwd = -_torso.Body.GlobalTransform.Basis.Z; + _impactor.GlobalPosition = chest + fwd * 0.5f + Vector3.Up * 0.05f; + _impactor.LinearVelocity = -fwd * 3f; + _impactor.Visible = true; + GD.Print($"BODY: impactor armed freeze={_impactor.Freeze} pos={_impactor.GlobalPosition} v={_impactor.LinearVelocity}"); + } + } + + private void FinishPairs() + { + float chestY = _torso.Body.GlobalPosition.Y; + bool handBelow = _handProbe.GlobalPosition.Y < chestY - 0.4f; + Record($"hand-config body falls through own chest (hand y {_handProbe.GlobalPosition.Y:F2}, chest y {chestY:F2})", handBelow); + Record("chest undisturbed while hand-config body falls through", _chestStillDuringHandDrop); + bool propContact = _chestDropDip > 0.0001f; + bool propBounded = _chestDropDip < 0.1f; + Record($"dropped ball transfers contact into chest (dip {_chestDropDip:F4} m)", propContact && propBounded); + _prop.LinearVelocity = Vector3.Zero; + _prop.GlobalPosition = new Vector3(-2f, 0.3f, 1f); + StartPhase(Phase.Track); + GD.Print("BODY: track phase — head walks a smooth path"); + } + + private void FinishImpact() + { + Record($"impact chest dip max {_maxImpactDip:F3} m (limit {ImpactDipLimit})", _maxImpactDip < ImpactDipLimit); + float recovered = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition); + Record($"impact recovery {recovered:F3} m (limit {RecoverLimit})", recovered < RecoverLimit); + Record($"impactor never passed through (min dist {_minImpactorDist:F3} m, limit {ImpactorMinDistLimit})", _minImpactorDist > ImpactorMinDistLimit); + Record($"impactor slowed after hit (|v| {_impactorSpeedAfterHit:F2} m/s, limit {ImpactorSpeedAfterHitLimit})", + _impactorSpeedAfterHit >= 0f && _impactorSpeedAfterHit < ImpactorSpeedAfterHitLimit); + StartPhase(Phase.Snap); + _head.GlobalPosition += new Vector3(2.5f, 0f, 0f); + GD.Print("BODY: snap phase — head teleported 2.5 m"); + } + + private void FinishUpright() + { + float up = _torso.Body.GlobalTransform.Basis.Y.Dot(Vector3.Up); + float uprightDeg = Mathf.RadToDeg(Mathf.Acos(Mathf.Clamp(up, -1f, 1f))); + float yawErr = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis)); + Record($"self-right to upright {uprightDeg:F1} deg (limit {UprightLimitDeg})", uprightDeg < UprightLimitDeg); + Record($"yaw held through tumble {yawErr:F1} deg (limit {YawHoldLimitDeg})", yawErr < YawHoldLimitDeg); + Finish(); + } + + private static float YawErrorDeg(Basis bodyBasis, Basis headBasis) + { + Vector3 bodyFwd = -bodyBasis.Z; + Vector3 headFwd = -headBasis.Z; + bodyFwd.Y = 0f; + headFwd.Y = 0f; + if (bodyFwd.LengthSquared() < 0.0001f || headFwd.LengthSquared() < 0.0001f) + return 0f; + bodyFwd = bodyFwd.Normalized(); + headFwd = headFwd.Normalized(); + return Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Dot(Vector3.Up), bodyFwd.Dot(headFwd))); + } + + private void Record(string name, bool ok) + { + _resultNames.Add(name); + _resultOk.Add(ok); + GD.Print($"BODY: {name} → {(ok ? "PASS" : "FAIL")}"); + } + + private void Finish() + { + bool overall = true; + foreach (bool ok in _resultOk) + overall &= ok; + GD.Print($"BODY: overall {(overall ? "PASS — physical torso functional on Box3D" : "CHECK — inspect failures above")}"); + GetTree().Quit(); + } +} diff --git a/scripts/BodySpike.cs.uid b/scripts/BodySpike.cs.uid new file mode 100644 index 0000000..eec1d5a --- /dev/null +++ b/scripts/BodySpike.cs.uid @@ -0,0 +1 @@ +uid://bm24msqxk21rx diff --git a/scripts/PhysicsHands.cs b/scripts/PhysicsHands.cs index 44ecf92..f41e9a8 100644 --- a/scripts/PhysicsHands.cs +++ b/scripts/PhysicsHands.cs @@ -66,8 +66,8 @@ public partial class PhysicsHands : Node ContinuousCd = true, ContactMonitor = true, MaxContactsReported = 8, - CollisionLayer = 1, - CollisionMask = 1, + CollisionLayer = 0b1000, // layer 4 = player hands + CollisionMask = 0b1001, // world + hands; chest (layer 3) excluded both ways }; body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } }); MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = HandSize } }; diff --git a/scripts/PhysicsTorso.cs b/scripts/PhysicsTorso.cs new file mode 100644 index 0000000..562cbf6 --- /dev/null +++ b/scripts/PhysicsTorso.cs @@ -0,0 +1,165 @@ +using Godot; + +public partial class PhysicsTorso : Node +{ + private const float TorsoMass = 30f; + private const float MaxForce = 2400f; + private const float MaxSpeed = 6f; + public const float SnapDistance = 2f; + private const float TorsoDrop = 0.42f; + private const float TorsoBackOffset = 0.06f; + private const float HeadBackOffset = 0.06f; + public const float YawHandsWeight = 0.6f; + public const float YawForwardness = 0.3f; + public const float YawSmoothRate = 12f; + private const float OrientKp = 20f; + private const float OrientMaxSpin = 10f; + private const float OrientMaxAlpha = 150f; + + private static readonly Vector3 TorsoSize = new(0.22f, 0.5f, 0.13f); + + private static readonly Color BodyColor = new(0.4f, 0.45f, 0.55f); + + public Node3D HeadSource; + public RigidBody3D HandBodyLeft; + public RigidBody3D HandBodyRight; + public Vector3 ReachOffset; + + private XRCamera3D _camera; + private XRController3D _leftController; + private XRController3D _rightController; + private RigidBody3D _handL; + private RigidBody3D _handR; + private RigidBody3D _body; + + private Basis _yawBasis = Basis.Identity; + private Basis _torsoBasis = Basis.Identity; + private float _torsoYaw; + private bool _yawInitialized; + private bool _arrived; + private MeshInstance3D _mesh; + + public RigidBody3D Body => _body; + public Basis TorsoBasis => _body != null ? _body.GlobalTransform.Basis : _torsoBasis; + public Vector3 TargetPosition { get; private set; } + + public override void _Ready() + { + _body = new RigidBody3D + { + Name = "TorsoBody", + Mass = TorsoMass, + GravityScale = 0f, + ContinuousCd = true, + ContactMonitor = true, + MaxContactsReported = 8, + CollisionLayer = 0b100, // layer 3 = player body + CollisionMask = 0b101, // world + player body; hands (layer 4) excluded both ways + }; + _body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = TorsoSize } }); + _mesh = new MeshInstance3D { Mesh = new BoxMesh { Size = TorsoSize }, Visible = false }; + _mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = BodyColor, Roughness = 0.6f }); + _body.AddChild(_mesh); + AddChild(_body); + + if (HeadSource == null) + { + _camera = GetNode("../XROrigin3D/XRCamera3D"); + _leftController = GetNode("../XROrigin3D/LeftHand"); + _rightController = GetNode("../XROrigin3D/RightHand"); + _handL = PhysicsHands.GetBody(_leftController); + _handR = PhysicsHands.GetBody(_rightController); + } + GD.Print("PhysicsTorso: chase body spawned"); + } + + public override void _PhysicsProcess(double delta) + { + Node3D head = HeadSource != null ? HeadSource : (Node3D)_camera; + if (head == null || _body == null) + return; + float dt = (float)delta; + + _yawBasis = FlatYawBasis(head.GlobalTransform.Basis); + Vector3 headPos = head.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset; + UpdateTorsoYaw(headPos, dt); + Vector3 target = headPos + _torsoBasis * new Vector3(0f, -TorsoDrop, TorsoBackOffset) + ReachOffset; + TargetPosition = target; + + if (!_arrived && _body.GlobalPosition.DistanceTo(target) < 0.3f) + { + _arrived = true; + _mesh.Visible = true; + } + + if (_body.GlobalPosition.DistanceTo(target) > SnapDistance) + { + _body.GlobalTransform = new Transform3D(_torsoBasis, target); + _body.LinearVelocity = Vector3.Zero; + _body.AngularVelocity = Vector3.Zero; + return; + } + + Vector3 desiredVelocity = (target - _body.GlobalPosition) / dt; + if (desiredVelocity.Length() > MaxSpeed) + desiredVelocity = desiredVelocity.Normalized() * MaxSpeed; + Vector3 dv = desiredVelocity - _body.LinearVelocity; + Vector3 force = dv / dt * TorsoMass; + if (force.Length() > MaxForce) + 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 wErr = spinDesired - _body.AngularVelocity; + float maxDeltaW = OrientMaxAlpha * dt; + if (wErr.Length() > maxDeltaW) + wErr = wErr.Normalized() * maxDeltaW; + _body.AngularVelocity += wErr; + } + + private void UpdateTorsoYaw(Vector3 headPos, float delta) + { + float hmdYaw = YawOf(-_yawBasis.Z); + float targetYaw = hmdYaw; + if (_handL != null && _handR != null) + { + Vector3 toHands = (_handL.GlobalPosition + _handR.GlobalPosition) * 0.5f - headPos; + Vector3 horizontal = new(toHands.X, 0f, toHands.Z); + if (horizontal.LengthSquared() > 0.0001f) + { + float handsYaw = YawOf(horizontal.Normalized()); + float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / YawForwardness, 0f, 1f); + targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight); + } + } + if (!_yawInitialized) + { + _torsoYaw = hmdYaw; + _yawInitialized = true; + } + _torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * YawSmoothRate)); + Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw)); + _torsoBasis = Basis.LookingAt(fwd, Vector3.Up); + } + + private static Basis FlatYawBasis(Basis basis) + { + Vector3 fwd = -basis.Z; + fwd.Y = 0f; + if (fwd.LengthSquared() < 0.0001f) + return Basis.Identity; + return Basis.LookingAt(fwd.Normalized(), Vector3.Up); + } + + private static float YawOf(Vector3 forward) + { + return Mathf.Atan2(-forward.X, -forward.Z); + } +} diff --git a/scripts/PhysicsTorso.cs.uid b/scripts/PhysicsTorso.cs.uid new file mode 100644 index 0000000..7887881 --- /dev/null +++ b/scripts/PhysicsTorso.cs.uid @@ -0,0 +1 @@ +uid://cv6gyg2tf1qsg diff --git a/scripts/PropShop.cs b/scripts/PropShop.cs index cf95d9a..dc5ea5a 100644 --- a/scripts/PropShop.cs +++ b/scripts/PropShop.cs @@ -45,6 +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.AddChild(new CollisionShape3D { Position = new Vector3(0, 0.725f, -0.6f), @@ -95,6 +96,7 @@ public partial class PropShop : Node3D ContinuousCd = spec.Mass < 0.1f, ContactMonitor = true, MaxContactsReported = 4, + CollisionMask = 0b1101, // world + player body + hands }; body.AddToGroup("prop");