Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Body step 2: the player is a muscle-driven ragdoll (Project TX)

Browse Source 
PlayerRagdoll: 9 bodies, spine/neck ball joints, shoulder drives (girdle + muscle),
elbow hinges, torque-capped motor PDs toward the True Body, chest->hand guides,
implicit balance motor, mass-scaled ground control. PlayerRig: XR driver with
stick move / snap turn and Bonelab-style push-back (eye->camera sphere sweep).
Replaces the chase bodies in main.tscn; RagdollSpike regression (14 phases).

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed 2026-10-02 06:43:52 +03:00
1 parent d0b50fbc41
commit d83273dd2f
16 files changed
+1487 -28
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

AGENTS.md
+12 -6
View File
@@ -103,8 +103,9 @@ Four layers, bottom-up, applied per frame in this order:
project: chain budgets, screen-size/distance LOD, simplified colliders. If the per-bone C# project: chain budgets, screen-size/distance LOD, simplified colliders. If the per-bone C#
loop profiles hot, the hot loop moves to a GDExtension — profile first, don't pre-optimize. loop profiles hot, the hot loop moves to a GDExtension — profile first, don't pre-optimize.
Do NOT build anything that needs 6DOF/cone-twist joints between tracked bodies (the wrapper lacks SUPERSEDED for the player (2026-10-02): the fork now maps Generic6DOFJoint3D (ball/drive/weld), and the
them; velocity steering replaces them). Scene objects stay standard nodes (`RigidBody3D`, player body is a jointed muscle ragdoll (item 11). Layer 3 above describes the legacy chase bodies, which
remain valid for non-player things (and their spikes). Scene objects stay standard nodes (`RigidBody3D`,
`StaticBody3D`, `CharacterBody3D`) so arbitrary imported scene content works and the Jolt fallback `StaticBody3D`, `CharacterBody3D`) so arbitrary imported scene content works and the Jolt fallback
stays real. stays real.
@@ -289,10 +290,15 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x
gitignored `prefs.json` next to the executable (forced gate on launch, `Recalibrate()` via group gitignored `prefs.json` next to the executable (forced gate on launch, `Recalibrate()` via group
"calibration"), `BodyProfile` (Winter anthropometry, arm stored 1.5 cm short), shared `ArmSolver`, and the "calibration"), `BodyProfile` (Winter anthropometry, arm stored 1.5 cm short), shared `ArmSolver`, and the
TRUE BODY solver (`TrueBody.cs`: neck, chest yaw + crouch lean, lagging pelvis, moving shoulder girdle, arms) TRUE BODY solver (`TrueBody.cs`: neck, chest yaw + crouch lean, lagging pelvis, moving shoulder girdle, arms)
with a debug skeleton. Arm-length decisions: `research/vr-avatar-arm-length.md`. Next: step 2 = the with a debug skeleton. Arm-length decisions: `research/vr-avatar-arm-length.md`. **Step 2 DONE 2026-10-02
muscle-driven ragdoll player (ball joints + torque-capped motors toward the True Body), replacing the (awaiting owner playtest): the player IS a muscle-driven ragdoll** — `PlayerRagdoll.cs` (9 bodies, BALL/hinge/
chase-body chest/arms/hands; ground control; then legs. Owner scope: thumbstick locomotion only, no jump, DRIVE joints with torque-capped motor PDs toward the True Body, chest→hand guides, implicit balance, ground
avatar scaling and NPCs later. control) + `PlayerRig.cs` (XR driver: stick move/snap turn, Bonelab push-back via an eye→camera sphere sweep,
discrete origin jumps carry the body). It REPLACED PhysicsHands/PhysicsTorso/PhysicsArms/BodyRig in
`main.tscn` (legacy files kept for their spikes). Record + measured rules:
`research/ragdoll-player-step2.md` — **never put a light (< ~1 kg) body between two strong motors** (Box3D
one-iteration substeps oscillate). Regression `scenes/ragdoll_spike.tscn`. Next: procedural legs, 144 Hz,
climbing. Owner scope: thumbstick locomotion only, no jump, avatar scaling and NPCs later.
10. **Body physics Stage 2 — PHYSICAL ARMS, implemented 2026-10-02, awaiting owner playtest.** 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 `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 PD-chase their analytic 2-bone IK pose, solved each physics tick from the PHYSICAL chest
HANDOFF.md
+29 -3
View File
@@ -78,6 +78,22 @@ TERMINOLOGY (owner-corrected — use precisely):
head. Grip is suppressed while that hand prop-grabs (`PropGrabber.IsGrabbing`) or the panel is head. Grip is suppressed while that hand prop-grabs (`PropGrabber.IsGrabbing`) or the panel is
near (`WorldPanel.IsBlocking`). **All deltas and turn pivots in WORLD space** (tracking-space math 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. made turns orbit the map center — do not regress). Drives `XROrigin3D` directly.
- **PlayerRagdoll.cs + PlayerRig.cs (body step 2, 2026-10-02) — THE PLAYER BODY in `main.tscn`.** The player is a
muscle-driven ragdoll (Nimso's Project TX): 9 boxes (pelvis carrying the legs' mass, chest, head+neck, upper arm,
forearm, hand ×2; 60 kg), spine/neck BALL joints + elbow hinges + shoulder DRIVEs (girdle translation + shoulder
muscle in one joint) with torque-capped motor PDs (feed-forward + Kp·RotationError) toward the True Body pose; hands
pulled onto the controllers by a 40 N chest→hand guide; balance = implicit motor from a world anchor; ground control
= mass-scaled support PD on the pelvis while a ray finds ground. `PlayerRig` (XR driver) per physics tick:
discrete origin jumps (spawn/teleport) carry the body → left-stick move (head-relative 2.2 m/s) / right-stick snap
30° (`SmoothTurn` constant) → `OriginCorrection` (camera 1:1; a 3 cm sphere swept eye→camera pushes the view back
when geometry is in the way, free lead ≤ 0.2 m, floor follows the ground, airborne the view rides the body) →
True Body solve → `Step`. Sticks whose hand points at the menu don't move/turn (`WorldPanel.IsPointing`).
`PhysicsHands.GetBody` falls back to the ragdoll's hands and `PropGrabber` calls `PlayerRig.SetHeld` (arm↔prop
collision exceptions), so grabbing works unchanged. Hand ghosts at > 10 cm lag. Rebuilds on recalibration. Full
design + measured findings: `research/ragdoll-player-step2.md` (incl. the light-link oscillation rule).
Regression `scenes/ragdoll_spike.tscn` "RAGDOLL: overall PASS" (stand, reach, swings, stick + room-scale wall,
bar blocked/crouch under, lean over table, crate push, 12 kg close vs far, teleport + snap turn, random soak).
- **LEGACY (not in main.tscn since step 2; kept for their spikes): PhysicsHands / PhysicsTorso / PhysicsArms / BodyRig.**
- **PhysicsHands.cs** — physical palm bodies chasing the controller poses: 0.4 kg boxes (flat slab, - **PhysicsHands.cs** — physical palm bodies chasing the controller poses: 0.4 kg boxes (flat slab,
thumb-side face), gravity off, CCD, **layer 4 (player hands)/mask 1|4** — NOT colliding with the 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` chest (layer 3), excluded both ways. Chase = desired velocity `(target-pos)/dt`
@@ -356,6 +372,12 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29).
## 5. Known issues / open questions ## 5. Known issues / open questions
- **Ragdoll player (step 2) — unvalidated in-headset.** Known gaps (details in
`research/ragdoll-player-step2.md`): no legs yet; shoulder has no cone limit; grabs are props only (no
climbing on static geometry); strength values are human-scale guesses; physics still at the headset rate
(144 Hz not switched); legacy anchor-grab movement now fights gravity (the body falls, the view follows), so
it only pulls you along the ground; teleport markers still work (the body is carried).
- **Body Stage 1 chest collapse — ROOT-CAUSED + FIXED (2026-10-01), awaiting owner re-playtest.** - **Body Stage 1 chest collapse — ROOT-CAUSED + FIXED (2026-10-01), awaiting owner re-playtest.**
The first in-headset session NaN'd the chest ("head detaching from the body"). Cause was The first in-headset session NaN'd the chest ("head detaching from the body"). Cause was
SCRIPT MATH, not the solver: the orientation servo used `Quaternion.GetAngle()` = `2*acos(W)` SCRIPT MATH, not the solver: the orientation servo used `Quaternion.GetAngle()` = `2*acos(W)`
@@ -500,9 +522,13 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29).
## 7. Next steps (in order) ## 7. Next steps (in order)
0. **Owner playtest of body step 1** — T-pose calibration (forced on first launch; delete `prefs.json` to redo, 0. **Owner playtest of body step 2 (the ragdoll player)** — stand, reach, push props, grab/hold (strength is now
or the menu's Recalibrate), arm length should now match; judge the cyan True Body skeleton (shoulders that human-scale: heavy things only close to the body), walk with the left stick into walls/the table, snap turn
shrug/reach, crouch lean, hip sway, elbows). Then body step 2 = the muscle ragdoll on the 6-DOF joints. with the right, crouch. Read the log after ("PlayerRagdoll: ... snapped" = a safety reset). Tuning knobs:
`PlayerRagdoll` strength/gain constants, `PlayerRig.MoveSpeed/SmoothTurn`. Step 1 (calibration, True Body)
was validated by the owner ("seems normal"); `TrueBody.ShowDebug` is off now that the ragdoll is the visual.
Next body steps: procedural legs, 144 Hz (needs physics interpolation + origin moves in physics ticks),
climbing (weld to static), one global strength scalar.
1. **Owner re-playtest of the body (chest fix + Stage 2 arms)** — the Stage-1 collapse is 1. **Owner re-playtest of the body (chest fix + Stage 2 arms)** — the Stage-1 collapse is
fixed (`collapse_spike` 49/49); feel is unvalidated in-headset. Read the log after. fixed (`collapse_spike` 49/49); feel is unvalidated in-headset. Read the log after.
2. **Locomotion decision** — owner report 2026-10-01: two-hand anchor turning broken (not 2. **Locomotion decision** — owner report 2026-10-01: two-hand anchor turning broken (not
research/ragdoll-player-step2.md
+65
View File
@@ -0,0 +1,65 @@
# Body step 2 — the muscle-driven ragdoll player (2026-10-02)
Implementation record for Nimso's Project TX player rebuilt on Box3D (design source:
`nimso-project-tx.md` §2; joint mapping: `godot-box3d-6dof.md`). Code: `scripts/PlayerRagdoll.cs` (input-agnostic
body), `scripts/PlayerRig.cs` (XR driver + stick locomotion), regression `scenes/ragdoll_spike.tscn`
("RAGDOLL: overall PASS", 14 phases). It replaces PhysicsHands / PhysicsTorso / PhysicsArms / BodyRig in
`main.tscn`. Those scripts stay for their own spikes.
## What was built
| Part | Implementation | Why |
|---|---|---|
| Bodies | 9 boxes, each with its origin at its centre of mass: pelvis (pelvis + abdomen + both legs' Dempster mass, 36 kg at 60 kg total), chest 13 kg, head + neck 4.9 kg, upper arm 1.7, forearm 1.0, hand 0.36 | Legs aren't physical yet (Nimso's leg collision is off by default too), but their mass belongs to the body the ground carries |
| Spine, neck | `Generic6DOFJoint3D` BALL (cone/twist, twist axis = up) + muscle motor | Local muscles, Nimso: "each bone drives itself" |
| Shoulder | `Generic6DOFJoint3D` DRIVE chest→upper arm. The linear velocity motor (2000 N) holds the humeral head on the chest point + the True Body girdle offset; the angular velocity motor (45 N·m) is the shoulder muscle | One joint carries both the moving girdle (shrug/protraction = real reach) and the muscle. See "light links" below |
| Elbow | `HingeJoint3D` 0..150° with a muscle motor (45 N·m) on the flexion angle | A real hinge can't flip or hyper-extend |
| Wrist | BALL, cone 80°, twist ±90°, no motor | The hand guide orients the hand |
| Hand guide | DRIVE chest→hand. Linear velocity motor 40 N with controller-velocity feed-forward, angular 25 N·m | "Tracking is a guide". The reaction goes into the chest, so pushing a wall pushes you |
| Balance | DRIVE from a shapeless world anchor to the pelvis, angular motor only (400 N·m grounded, 30% airborne) | Upright/yaw hold. See "explicit torques" below |
| Ground control | Ray down from the pelvis. If ground is within hip height + 0.3 m (normal.y > 0.6): critically damped 4 Hz PD on the pelvis toward its pose height above the actual ground, with gravity compensation for the whole body mass. Support is clamped to [0, 2 × weight], horizontal force to friction × support | Nimso's "leg strength" scales with mass and only pushes. No ground → falls |
| Muscle law | Motor target velocity = feed-forward (the target's own relative spin) + Kp·RotationError(target, current) in joint frame A, capped at 15 rad/s; force/torque cap = strength | Proven in `sixdof_spike`. Feed-forward keeps moving poses from lagging by speed/Kp |
| Targets | From `TrueBodySolver`: trunk rebuilt top-down (upper chest → lumbar → hips, hips take 50% of the crouch lean). Head = camera orientation tilted so the body's eye lands on the camera. Arms = ArmSolver shoulder/elbow/wrist; hinge axis = Pole × arm direction | A pose the physical chain can actually reach |
| Camera / play space | Camera stays 1:1. Each tick the driver applies `OriginCorrection`: a 3 cm sphere swept from the head body's eye to the camera. If geometry is in the way, the view is put back at the free point (walls stop both stick and real walking). If the path is free, the camera may lead the body by up to 0.2 m (leaning over a table whose edge holds your hips), beyond that it follows. Grounded: horizontal only, the floor eases toward the ground under the pelvis. Airborne: 3D (falls carry the view) | Owner: Bonelab push-back, no fade |
| Discrete moves | Origin jumps > 0.5 m or > 0.1 rad in one tick (spawn, teleport markers) and snap turns carry the body with `TeleportBy` + `TrueBodySolver.Shift` | No body flung across the gap |
| Locomotion | Left stick head-relative 2.2 m/s (8 m/s² accel, no air control); right stick snap 30° (`PlayerRig.SmoothTurn` → 2.5 rad/s). A stick whose hand points at the menu scrolls it instead | Owner scope: Boneworks-style stick only, no jump |
## Measured findings (RagdollSpike, Box3D fork, 90 Hz, 4 substeps)
1. **Light links between two strong motors oscillate.** The first build had a 0.6 kg neck link (two neck joints)
and a 0.8 kg clavicle body per arm (chest→clavicle drive + clavicle→upper-arm ball). The neck pitch oscillated
with growing amplitude (±11 rad/s head spin while standing still). Raising the shoulder to 200 N·m made the arms
*worse* (0.94 m hand lag vs 0.47 m). Box3D runs one solver iteration per substep, and a light body squeezed
between two motors with large caps doesn't converge. **Fix: no light intermediate bodies.** The neck merged into
the head (single joint at the neck base); the clavicle was replaced by one shoulder DRIVE carrying both the girdle
translation and the muscle. Result: standing error ~0, swing lag 0.13 m, random-motion eye error 2.3 cm. Rule
for the legs and anything later: keep every body that sits between two motors ≳ 1 kg, or merge it.
2. **Explicit world torques on the pelvis vs an implicit motor.** The explicit 3 Hz PD torque balance looked fine
until the arms got strong. With the light links gone, the implicit balance motor is stable and holds the pelvis
rigidly against arm reaction torques.
3. **Hand-guide strength is the "cheat budget".** With a 120 N guide the arms looked perfect, but 12 kg at arm's
length sagged only 2 cm (Nimso: it must not be holdable). At 40 N guide + 45 N·m shoulder/elbow (≈ human):
12 kg held close sags 1.3 cm, at arm's length 25 cm. Fast swings still track (0.13 m peak lag at 1.5 Hz ±0.25 m).
4. **Test poses must be anatomically possible.** Controllers pointing forward with the arms out sideways need a 90°
wrist bend. The old strong guide hid that; with honest strength the wrist limit wins. RagdollSpike's reach poses
now aim the controllers along the arm.
5. **Unreachable targets stop at the arm's end.** That is correct, not a bug: a push target 0.91 m from a 0.57 m
arm leaves the hands at full reach.
6. Hinge sign: positive `HingeJoint3D` motor velocity rotates the child about the joint's +Z (spike phase 0 measures
it; `PlayerRagdoll.HingeSign`).
## Not done yet / known gaps
- **Legs**: no physical or visual legs; the pelvis floats on ground control. Step 3 = procedural legs (Nimso:
"feet don't hold the player up" — visuals + optional collision).
- **Shoulder has no cone limit** (Box3D's motor joint has none). A strong external force can bend the arm into the
chest; muscles return it. If it shows, add a BALL limit via a second, collision-only constraint, or a clavicle
heavy enough (≥ 1 kg) to be stable.
- **Climbing**: grabs are props only (`PropGrabber`); welding to static geometry is not wired. The physics would
allow it: the hand guide reacts into the chest.
- **Strength values are guesses** at human scale (Nimso's are unpublished). One global strength scalar (Nimso's
"Crysis mode") is not exposed yet.
- **144 Hz**: not switched. Physics runs at the headset rate. Decoupling it needs physics interpolation, and today
`AnchorMoveMode`/`Locomotion` move the XR origin in `_Process`, which would jitter under interpolation.
- **Anchor-grab / teleport legacy locomotion** still drive the origin. Teleports carry the body. Grab-flying upward
now fights gravity (the body falls and the view follows it), so it effectively only pulls you along the ground.
scenes/main.tscn
+4 -16
View File
@@ -1,4 +1,4 @@
[gd_scene load_steps=23 format=3] [gd_scene load_steps=20 format=3]
[ext_resource type="Script" path="res://scripts/Main.cs" id="1_main"] [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"] [ext_resource type="PackedScene" path="res://scenes/teleport_marker.tscn" id="2_marker"]
@@ -7,12 +7,9 @@
[ext_resource type="Script" path="res://scripts/AnchorMoveMode.cs" id="5_anchormove"] [ext_resource type="Script" path="res://scripts/AnchorMoveMode.cs" id="5_anchormove"]
[ext_resource type="Script" path="res://scripts/PropShop.cs" id="6_propshop"] [ext_resource type="Script" path="res://scripts/PropShop.cs" id="6_propshop"]
[ext_resource type="Script" path="res://scripts/PropGrabber.cs" id="7_propgrabber"] [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"]
[ext_resource type="Script" path="res://scripts/PhysicsArms.cs" id="11_physicsArms"]
[ext_resource type="Script" path="res://scripts/Calibration.cs" id="12_calibration"] [ext_resource type="Script" path="res://scripts/Calibration.cs" id="12_calibration"]
[ext_resource type="Script" path="res://scripts/TrueBody.cs" id="13_truebody"] [ext_resource type="Script" path="res://scripts/TrueBody.cs" id="13_truebody"]
[ext_resource type="Script" path="res://scripts/PlayerRig.cs" id="14_playerrig"]
[sub_resource type="ProceduralSkyMaterial" id="ProceduralSkyMaterial_sky"] [sub_resource type="ProceduralSkyMaterial" id="ProceduralSkyMaterial_sky"]
sky_horizon_color = Color(0.66, 0.72, 0.81, 1) sky_horizon_color = Color(0.66, 0.72, 0.81, 1)
@@ -115,23 +112,14 @@ script = ExtResource("6_propshop")
[node name="PropGrabber" type="Node" parent="."] [node name="PropGrabber" type="Node" parent="."]
script = ExtResource("7_propgrabber") 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="PhysicsArms" type="Node" parent="."]
script = ExtResource("11_physicsArms")
[node name="Calibration" type="Node" parent="."] [node name="Calibration" type="Node" parent="."]
script = ExtResource("12_calibration") script = ExtResource("12_calibration")
[node name="TrueBody" type="Node3D" parent="."] [node name="TrueBody" type="Node3D" parent="."]
script = ExtResource("13_truebody") script = ExtResource("13_truebody")
[node name="BodyRig" type="Node3D" parent="."] [node name="PlayerRig" type="Node" parent="."]
script = ExtResource("9_bodyrig") script = ExtResource("14_playerrig")
[node name="FadeLayer" type="CanvasLayer" parent="."] [node name="FadeLayer" type="CanvasLayer" parent="."]
scenes/ragdoll_spike.tscn
+6
View File
@@ -0,0 +1,6 @@
[gd_scene load_steps=2 format=3]
[ext_resource type="Script" path="res://scripts/RagdollSpike.cs" id="1_spike"]
[node name="RagdollSpike" type="Node3D"]
script = ExtResource("1_spike")
scripts/ArmSolver.cs
+5 -1
View File
@@ -20,6 +20,9 @@ public sealed class ArmSolver
public Vector3 Elbow { get; private set; } public Vector3 Elbow { get; private set; }
public Vector3 Wrist { get; private set; } public Vector3 Wrist { get; private set; }
// Unit vector from the arm axis toward the elbow (defined even for a straight arm); the
// elbow hinge axis is Pole × (shoulder→wrist).
public Vector3 Pole { get; private set; } = Vector3.Back;
// side = the arm's outward direction (−torso X for the left arm, +X for the right). // side = the arm's outward direction (−torso X for the left arm, +X for the right).
// handBasis = the controller's basis (null → torso-frame reference only). // handBasis = the controller's basis (null → torso-frame reference only).
@@ -78,7 +81,8 @@ public sealed class ArmSolver
_poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta); _poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta);
Vector3 pole = refN.Rotated(dir, _poleAngle); Vector3 pole = refN.Rotated(dir, _poleAngle);
Elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius; Pole = CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance);
Elbow = center + Pole * radius;
Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen; Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen;
} }
scripts/PhysicsHands.cs
+1 -1
View File
@@ -54,7 +54,7 @@ public partial class PhysicsHands : Node
public static RigidBody3D GetBody(XRController3D controller) public static RigidBody3D GetBody(XRController3D controller)
{ {
if (_instance == null) if (_instance == null)
return null; return PlayerRig.GetHandBody(controller); // the ragdoll player owns the hands
return _instance._left == controller ? _instance._bodies[0] : _instance._bodies[1]; return _instance._left == controller ? _instance._bodies[0] : _instance._bodies[1];
} }
scripts/PlayerRagdoll.cs
+644
View File
@@ -0,0 +1,644 @@
using Godot;
using System.Collections.Generic;
// Body step 2 — the player IS a muscle-driven ragdoll (Nimso's Project TX, research/nimso-project-tx.md
// §2): every bone drives ITSELF toward the True Body pose through torque-capped joint motors
// (never world-pinned), the hands are additionally pulled to the controllers by a force-capped
// guide that reacts into the chest, and "ground control" holds the pelvis up and carries the body
// after the head. Input-agnostic: a driver (PlayerRig in-game, RagdollSpike headless) solves the
// TrueBodySolver, then calls Step(); the camera stays 1:1 and the driver moves its play space by
// OriginCorrection() when the body is blocked (push-back) or stands on higher/lower ground.
//
// Bodies (boxes, origin = centre of mass): pelvis (+ the not-yet-physical legs' mass), chest,
// head (incl. neck), and per arm upper arm, forearm, hand. No light intermediate links: a 0.6 kg
// neck or 0.8 kg clavicle squeezed between two strong motors oscillates under Box3D's
// one-iteration-per-substep solver (measured: RagdollSpike, 2026-10-02).
// Joints (fork Generic6DOFJoint3D mapping, research/godot-box3d-6dof.md):
// spine, neck, wrist = BALL (cone/twist limits), spine/neck with muscle motors;
// shoulder = DRIVE chest→upper arm: linear motor holds the humeral head on the (moving) girdle
// point, angular motor = the shoulder muscle (no cone limit — Box3D's motor joint has none);
// elbow = HingeJoint3D (0..150°) with a muscle motor;
// hand guide = DRIVE chest→hand; balance = DRIVE world anchor→pelvis (angular only).
// All motors are PDs written as target velocities each tick; the force/torque limits are the strength.
public partial class PlayerRagdoll : Node3D
{
public const float BodyMass = 60f; // Nimso's character is 60 kg
// Muscles: PD gain (1/s), max commanded spin (rad/s), torque cap (N·m).
public const float SpineKp = 15f, SpineTorque = 400f;
public const float NeckKp = 20f, NeckTorque = 60f;
public const float ShoulderKp = 20f, ShoulderTorque = 45f; // ~human shoulder flexion strength
public const float ElbowKp = 20f, ElbowTorque = 45f;
public const float MuscleMaxSpin = 15f;
public const float GirdleKp = 30f, GirdleForce = 2000f; // shoulder drive, linear part
// Hand guide (chest→hand drive): pulls the hand onto the controller; capped so the arm,
// not the guide, carries real loads.
public const float HandGuideKp = 40f, HandGuideForce = 40f, HandGuideMaxSpeed = 10f;
public const float HandGuideAngularKp = 40f, HandGuideTorque = 25f;
// Ground control (world-space support on the pelvis, only while grounded).
public const float GroundFrequency = 4f; // Hz
public const float GroundDamping = 1f;
public const float LegStrength = 2f; // max support, × body weight
public const float GroundFriction = 1f; // horizontal force ≤ this × support
public const float GroundProbeExtra = 0.3f; // ground counts while within hip height + this
// Balance: an implicit torque-capped motor from a world anchor to the pelvis (explicit torques
// let the arms' reaction torques set the whole trunk swinging).
public const float BalanceKp = 10f;
public const float BalanceTorque = 400f;
public const float AirBalanceShare = 0.3f;
// Play-space reconciliation.
public const float CameraRadius = 0.03f; // sphere swept from the head's eye to the camera
public const float FreeSlack = 0.2f; // camera may lead an unobstructed (lagging/held) head by this much
public const float GroundFollowRate = 8f;
public const float ResetDistance = 1.5f; // pelvis this far from its target → snap the whole body to the pose
// Geometry.
public const float LumbarFraction = 0.35f; // lumbar joint height above the hips, × trunk
public const float PelvisLeanShare = 0.5f; // share of the crouch lean taken by the hips
public static readonly Vector3 HeadCenterFromEye = new(0f, 0.02f, 0.08f);
public static readonly Vector3 HeadSize = new(0.16f, 0.21f, 0.2f);
public static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
public const float WristOffset = TrueBodySolver.WristOffset;
public const float ElbowMaxFlex = 2.6f; // rad (~150°)
// Sign of HingeJoint3D's angle/motor for a positive rotation of the child about the joint's
// +Z (measured by RagdollSpike's "hinge sign" phase).
public static float HingeSign = 1f;
// Collision layers (AGENTS.md): 3 body, 4 hands, 5 arms.
private const uint BodyLayer = 0b100, HandLayer = 0b1000, ArmLayer = 0b10000;
private const uint WorldMask = 0b1;
private sealed class Ball
{
public Generic6DofJoint3D Joint;
public RigidBody3D Parent, Child;
public Quaternion FrameA, FrameB; // joint frame relative to each body
public float Kp;
public Quaternion PrevTarget;
public bool HasPrev;
}
private sealed class Drive
{
public Generic6DofJoint3D Joint;
public Quaternion FrameA, FrameB;
public Vector3 AnchorA, AnchorB; // anchor point, parent-local / child-local
public Quaternion PrevTarget;
public bool HasPrev;
}
private sealed class Arm
{
public RigidBody3D Upper, Fore, Hand;
public Ball Wrist;
public HingeJoint3D Elbow;
public Drive Shoulder, Guide;
public float UpperLen, ForeLen;
public RigidBody3D Held;
public float PrevFlexTarget = -1f;
}
public RigidBody3D Pelvis { get; private set; }
public RigidBody3D Chest { get; private set; }
public RigidBody3D Head { get; private set; }
public bool Built { get; private set; }
public bool Grounded { get; private set; }
public float GroundY { get; private set; }
public Vector3 Eye => Head.GlobalTransform * _eyeLocal;
public BodyProfile Profile { get; private set; }
public int Snaps { get; private set; } // safety resets (NaN / too far) — should stay 0
private readonly Arm[] _arms = { new(), new() };
private readonly List<RigidBody3D> _bodies = new();
private Ball _spine, _neck;
private Vector3 _neckBaseToEye; // head frame, canonical (neck straight)
private float _trunk, _lumbar, _neckLen;
private Vector3 _eyeLocal;
private Vector3 _pelvisCenterLocal, _chestCenterLocal, _headCenterLocal; // from each body's proximal joint, in its basis
private readonly Dictionary<RigidBody3D, Transform3D> _targets = new();
private readonly Vector3[] _girdle = new Vector3[2]; // True Body shoulder offset, chest frame
private Vector3 _pelvisTargetPrev;
private bool _havePelvisPrev;
public RigidBody3D Hand(int side) => Built ? _arms[side].Hand : null;
public RigidBody3D UpperArm(int side) => Built ? _arms[side].Upper : null;
public RigidBody3D Forearm(int side) => Built ? _arms[side].Fore : null;
public IReadOnlyList<RigidBody3D> Bodies => _bodies;
// ---------------------------------------------------------------- build
public void Build(BodyProfile p, TrueBodySolver pose, Transform3D left, Transform3D right)
{
Clear();
Profile = p;
_trunk = TrueBodySolver.TrunkRatio * p.Stature;
_lumbar = LumbarFraction * _trunk;
_neckLen = Mathf.Max(0.05f, p.ShoulderDrop + TrueBodySolver.EyeToNeckPivot.Y - TrueBodySolver.NeckBaseAboveShoulders);
float m = BodyMass;
float s = p.Stature / 1.75f;
// Canonical pose (standing, level head, arms straight down-forward-out) in frame C at the
// current pelvis: joint cones/twists are centred on it and the elbow hinge's zero is a
// straight arm. The body is then moved onto the live pose.
Transform3D c = new(pose.YawBasis, pose.PelvisTarget);
Vector3 hip = Vector3.Zero;
Vector3 lumbar = new(0f, _lumbar, 0f);
Vector3 upperChest = new(0f, _trunk, 0f);
Vector3 neckBase = upperChest + new Vector3(0f, TrueBodySolver.NeckBaseAboveShoulders,
-(TrueBodySolver.ShouldersBehindEyes - TrueBodySolver.EyeToNeckPivot.Z));
Vector3 neckPivot = neckBase + new Vector3(0f, _neckLen, 0f);
_eyeLocal = -TrueBodySolver.EyeToNeckPivot; // head body origin is set to its centre below
Vector3 headCenterFromPivot = _eyeLocal + HeadCenterFromEye;
Vector3 pelvisSize = new(0.17f * p.Stature, _lumbar + 0.06f, 0.18f * s);
_pelvisCenterLocal = new Vector3(0f, _lumbar * 0.5f - 0.03f, 0f);
float chestHeight = _trunk - _lumbar + TrueBodySolver.NeckBaseAboveShoulders;
Vector3 chestSize = new(2f * p.ShoulderHalfWidth * 0.75f, chestHeight, 0.2f * s);
_chestCenterLocal = new Vector3(0f, chestHeight * 0.5f, 0f);
_headCenterLocal = headCenterFromPivot;
_eyeLocal -= headCenterFromPivot; // eye relative to the head body's centre
Color skin = new(0.85f, 0.72f, 0.6f), shirt = new(0.3f, 0.45f, 0.7f), pants = new(0.25f, 0.27f, 0.32f);
Pelvis = MakeBody("Pelvis", new BoxShape3D { Size = pelvisSize }, (0.142f + 0.139f + 0.322f) * m, BodyLayer, WorldMask, pants,
c * new Transform3D(Basis.Identity, hip + _pelvisCenterLocal));
Chest = MakeBody("Chest", new BoxShape3D { Size = chestSize }, 0.216f * m, BodyLayer, WorldMask, shirt,
c * new Transform3D(Basis.Identity, lumbar + _chestCenterLocal));
Head = MakeBody("Head", new BoxShape3D { Size = HeadSize * s }, 0.081f * m, BodyLayer, WorldMask, skin,
c * new Transform3D(Basis.Identity, neckPivot + headCenterFromPivot));
// The neck: a box hanging under the head, same body.
Vector3 neckCenter = (neckBase + neckPivot) * 0.5f - (neckPivot + headCenterFromPivot);
Head.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter });
MeshInstance3D neckMesh = new() { Mesh = new BoxMesh { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter };
neckMesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = skin, Roughness = 0.6f });
Head.AddChild(neckMesh);
_neckBaseToEye = neckPivot - TrueBodySolver.EyeToNeckPivot - neckBase;
// World anchor for the balance motor (no shape, linear motor force 0 = position free).
StaticBody3D anchor = new() { Name = "BalanceAnchor", CollisionLayer = 0, CollisionMask = 0 };
AddChild(anchor);
_balance = MakeDrive(anchor, Pelvis, Pelvis.GlobalTransform, 0f, BalanceTorque);
_balanceCap = BalanceTorque;
_spine = MakeBall(Pelvis, Chest, c * lumbar, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(60f), Mathf.DegToRad(35f), SpineKp, SpineTorque);
_neck = MakeBall(Chest, Head, c * neckBase, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(75f), Mathf.DegToRad(85f), NeckKp, NeckTorque);
for (int side = 0; side < 2; side++)
{
Arm a = _arms[side];
float sign = side == 0 ? -1f : 1f;
a.UpperLen = p.UpperArm;
a.ForeLen = p.Forearm;
Vector3 shoulder = upperChest + new Vector3(sign * p.ShoulderHalfWidth, 0f, 0f);
Vector3 dir = new Vector3(sign * 0.42f, -0.74f, -0.47f).Normalized();
Vector3 reference = Vector3.Down * 2f + Vector3.Back * 0.8f + new Vector3(sign * 0.3f, 0f, 0f);
Vector3 pole = (reference - dir * reference.Dot(dir)).Normalized();
Vector3 hinge = pole.Cross(dir).Normalized();
Basis bone = BoneBasis(dir, hinge);
Vector3 elbow = shoulder + dir * a.UpperLen;
Vector3 wrist = elbow + dir * a.ForeLen;
Vector3 handX = (Vector3.Right - dir * dir.X).Normalized();
Basis handBasis = new(handX, (-dir).Cross(handX), -dir);
Vector3 handOrigin = wrist + dir * WristOffset;
string n = side == 0 ? "Left" : "Right";
a.Upper = MakeBody(n + "UpperArm", new BoxShape3D { Size = new Vector3(0.085f * s, a.UpperLen, 0.085f * s) }, 0.028f * m, ArmLayer, WorldMask, shirt,
c * new Transform3D(bone, shoulder + dir * (a.UpperLen * 0.5f)));
a.Fore = MakeBody(n + "Forearm", new BoxShape3D { Size = new Vector3(0.07f * s, a.ForeLen - 0.01f, 0.07f * s) }, 0.016f * m, ArmLayer, WorldMask, skin,
c * new Transform3D(bone, elbow + dir * (a.ForeLen * 0.5f)));
a.Hand = MakeBody(n + "Hand", new BoxShape3D { Size = HandSize }, 0.006f * m, HandLayer, WorldMask | HandLayer,
side == 0 ? new Color(0.9f, 0.3f, 0.35f) : new Color(0.3f, 0.5f, 0.9f), c * new Transform3D(handBasis, handOrigin));
a.Hand.ContinuousCd = true;
a.Hand.ContactMonitor = true;
a.Hand.MaxContactsReported = 8;
a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(Basis.Identity, shoulder), GirdleForce, ShoulderTorque);
a.Elbow = MakeHinge(a.Upper, a.Fore, c * elbow, c.Basis * new Basis(dir, hinge.Cross(dir), hinge));
a.Wrist = MakeBall(a.Fore, a.Hand, c * wrist, c.Basis * TwistAlong(dir), Mathf.DegToRad(80f), Mathf.DegToRad(90f), 0f, 0f);
a.Guide = MakeDrive(Chest, a.Hand, c * new Transform3D(handBasis, handOrigin), HandGuideForce, HandGuideTorque);
}
Built = true;
_havePelvisPrev = false;
SnapToPose(pose, left, right);
GD.Print($"PlayerRagdoll: built for stature {p.Stature:F2} m (arm {p.UpperArm:F3}+{p.Forearm:F3} m, {_bodies.Count} bodies)");
}
public void Clear()
{
foreach (Node child in GetChildren())
child.QueueFree();
foreach (Node child in GetChildren())
RemoveChild(child);
_bodies.Clear();
_targets.Clear();
for (int i = 0; i < 2; i++)
_arms[i] = new Arm();
Built = false;
}
// Twist axis (joint X) along `dir`.
private static Basis TwistAlong(Vector3 dir)
{
Vector3 helper = Mathf.Abs(dir.Y) < 0.9f ? Vector3.Up : Vector3.Right;
Vector3 z = dir.Cross(helper).Normalized();
return new Basis(dir, z.Cross(dir), z);
}
// Segment basis: Y along the bone, X = elbow hinge axis.
private static Basis BoneBasis(Vector3 boneDir, Vector3 hinge)
{
Vector3 x = (hinge - boneDir * hinge.Dot(boneDir)).Normalized();
return new Basis(x, boneDir, x.Cross(boneDir));
}
private RigidBody3D MakeBody(string name, Shape3D shape, float mass, uint layer, uint mask, Color color, Transform3D at)
{
RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at };
body.AddChild(new CollisionShape3D { Shape = shape });
if (shape is BoxShape3D box && layer != 0)
{
MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = box.Size } };
mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f });
body.AddChild(mesh);
}
AddChild(body);
_bodies.Add(body);
return body;
}
private static Quaternion Rot(Node3D n) => n.GlobalTransform.Basis.Orthonormalized().GetRotationQuaternion();
// Configure BEFORE AddChild (the node pushes everything to the server on enter-tree).
private Ball MakeBall(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame, float cone, float twist, float kp, float torque)
{
Generic6DofJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -twist);
j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, twist);
foreach (var set in new System.Action<Generic6DofJoint3D.Param, float>[] { j.SetParamY, j.SetParamZ })
{
set(Generic6DofJoint3D.Param.AngularLowerLimit, -cone);
set(Generic6DofJoint3D.Param.AngularUpperLimit, cone);
}
if (torque > 0f)
{
j.SetFlagX(Generic6DofJoint3D.Flag.EnableMotor, true);
j.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, true);
j.SetFlagZ(Generic6DofJoint3D.Flag.EnableMotor, true);
j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
}
AddChild(j);
Quaternion jq = frame.Orthonormalized().GetRotationQuaternion();
return new Ball { Joint = j, Parent = parent, Child = child, FrameA = Rot(parent).Inverse() * jq, FrameB = Rot(child).Inverse() * jq, Kp = kp };
}
private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque)
{
Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
foreach (var flag in new System.Action<Generic6DofJoint3D.Flag, bool>[] { j.SetFlagX, j.SetFlagY, j.SetFlagZ })
{
flag(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
flag(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
flag(Generic6DofJoint3D.Flag.EnableLinearMotor, true);
flag(Generic6DofJoint3D.Flag.EnableMotor, true);
}
foreach (var param in new System.Action<Generic6DofJoint3D.Param, float>[] { j.SetParamX, j.SetParamY, j.SetParamZ })
{
param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force);
param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
}
AddChild(j);
Quaternion jq = at.Basis.Orthonormalized().GetRotationQuaternion();
return new Drive
{
Joint = j,
FrameA = Rot(parent).Inverse() * jq,
FrameB = Rot(child).Inverse() * jq,
AnchorA = parent.GlobalTransform.AffineInverse() * at.Origin,
AnchorB = child.GlobalTransform.AffineInverse() * at.Origin,
};
}
private HingeJoint3D MakeHinge(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame)
{
HingeJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
float lo = HingeSign > 0f ? 0f : -ElbowMaxFlex;
float hi = HingeSign > 0f ? ElbowMaxFlex : 0f;
j.SetFlag(HingeJoint3D.Flag.UseLimit, true);
j.SetParam(HingeJoint3D.Param.LimitLower, lo);
j.SetParam(HingeJoint3D.Param.LimitUpper, hi);
j.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
j.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque);
AddChild(j);
return j;
}
// ---------------------------------------------------------------- pose targets
// World transform every body should have in the True Body pose. The trunk is rebuilt top-down
// through the physical chain (upper chest → lumbar → hips) so a leaning chest doesn't drag
// the head off the camera; the hips take part of the crouch lean (hip hinge).
private void ComputeTargets(TrueBodySolver s, Transform3D left, Transform3D right)
{
Basis chest = s.ChestBasis;
Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * PelvisLeanShare);
Vector3 lumbar = s.UpperChest - chest.Y * (_trunk - _lumbar);
Vector3 hip = lumbar - pelvis.Y * _lumbar;
_targets[Pelvis] = new Transform3D(pelvis, hip + pelvis * _pelvisCenterLocal);
_targets[Chest] = new Transform3D(chest, lumbar + chest * _chestCenterLocal);
// Head + neck pivot at the neck base: orient like the camera, tilted by the small extra
// rotation that puts the body's eye on the camera (the True Body bends the neck at two points).
Vector3 eyeDir = s.HeadBasis * _neckBaseToEye;
Vector3 wantDir = s.Eye - s.NeckBase;
Quaternion tilt = eyeDir.LengthSquared() > 1e-8f && wantDir.LengthSquared() > 1e-8f
? new Quaternion(eyeDir.Normalized(), wantDir.Normalized()) : Quaternion.Identity;
Basis head = new Basis(tilt) * s.HeadBasis;
_targets[Head] = new Transform3D(head, s.NeckBase + head * (_neckBaseToEye - _eyeLocal));
for (int side = 0; side < 2; side++)
{
Arm a = _arms[side];
Vector3 sh = s.Shoulder[side], el = s.Elbow[side], wr = s.Wrist[side];
Vector3 armDir = wr - sh;
armDir = armDir.LengthSquared() > 1e-8f ? armDir.Normalized() : chest * Vector3.Down;
Vector3 hinge = s.Pole[side].Cross(armDir);
hinge = hinge.LengthSquared() > 1e-8f ? hinge.Normalized() : chest.X;
Vector3 up = (el - sh).Normalized();
Vector3 fore = (wr - el).Normalized();
_girdle[side] = chest.Transposed() * (sh - (s.UpperChest + chest.X * ((side == 0 ? -1f : 1f) * Profile.ShoulderHalfWidth)));
_targets[a.Upper] = new Transform3D(BoneBasis(up, hinge), sh + up * (a.UpperLen * 0.5f));
_targets[a.Fore] = new Transform3D(BoneBasis(fore, hinge), el + fore * (a.ForeLen * 0.5f));
_targets[a.Hand] = (side == 0 ? left : right).Orthonormalized();
}
}
public Transform3D Target(RigidBody3D body) => _targets.TryGetValue(body, out Transform3D t) ? t : body.GlobalTransform;
// ---------------------------------------------------------------- per tick
public void Step(TrueBodySolver s, Transform3D left, Transform3D right, Vector3 leftVelocity, Vector3 rightVelocity, float floorY, float dt)
{
if (!Built)
return;
ComputeTargets(s, left, right);
foreach (RigidBody3D b in _bodies)
{
if (BodyMath.ResetIfNonFinite(b, Target(b), "PlayerRagdoll"))
{
Snaps++;
SnapToPose(s, left, right);
return;
}
}
Transform3D pelvisTarget = _targets[Pelvis];
if (Pelvis.GlobalPosition.DistanceTo(pelvisTarget.Origin) > ResetDistance)
{
GD.Print("PlayerRagdoll: body too far from the pose, snapped to it");
Snaps++;
SnapToPose(s, left, right);
return;
}
Muscle(_spine, _targets[Pelvis].Basis, _targets[Chest].Basis, dt);
Muscle(_neck, _targets[Chest].Basis, _targets[Head].Basis, dt);
for (int side = 0; side < 2; side++)
{
Arm a = _arms[side];
ShoulderDrive(a, side, dt);
ElbowMuscle(a, dt);
HandGuide(a, _targets[a.Hand], side == 0 ? leftVelocity : rightVelocity);
}
GroundControl(pelvisTarget, floorY, dt);
}
// Muscle = PD on the joint's relative rotation toward the relative rotation of the two TARGET
// bases (a local pose, not a world pin), written as the motor's target velocity in frame A.
// Feed-forward = the target's own relative spin, so a moving pose isn't chased with a lag of
// speed/Kp.
private static void Muscle(Ball b, Basis parentTarget, Basis childTarget, float dt)
{
Quaternion current = (Rot(b.Parent) * b.FrameA).Inverse() * (Rot(b.Child) * b.FrameB);
Quaternion target = (parentTarget.Orthonormalized().GetRotationQuaternion() * b.FrameA).Inverse()
* (childTarget.Orthonormalized().GetRotationQuaternion() * b.FrameB);
Vector3 ff = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
b.PrevTarget = target;
b.HasPrev = true;
Vector3 w = (ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin);
SetAngularVelocity(b.Joint, w);
}
private static void SetAngularVelocity(Generic6DofJoint3D j, Vector3 w)
{
j.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
j.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
}
private static void SetLinearVelocity(Generic6DofJoint3D j, Vector3 v)
{
j.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X);
j.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y);
j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z);
}
// Elbow flexion (0 = straight) about the upper arm's hinge axis.
public float ElbowFlex(int side)
{
Arm a = _arms[side];
Basis u = a.Upper.GlobalTransform.Basis.Orthonormalized();
Vector3 f = a.Fore.GlobalTransform.Basis.Orthonormalized().Y;
return Mathf.Atan2(u.X.Dot(u.Y.Cross(f)), u.Y.Dot(f));
}
private void ElbowMuscle(Arm a, float dt)
{
int side = a == _arms[0] ? 0 : 1;
Vector3 up = _targets[a.Upper].Basis.Y, fore = _targets[a.Fore].Basis.Y;
float target = Mathf.Acos(Mathf.Clamp(up.Dot(fore), -1f, 1f));
float ff = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f;
a.PrevFlexTarget = target;
float w = Mathf.Clamp(ff + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin);
a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w);
}
// Shoulder = one drive: its linear motor keeps the humeral head on the chest's shoulder point
// shifted by the True Body girdle offset (shrug/protraction), its angular motor is the
// shoulder muscle (same PD + feed-forward as Muscle, parent = chest).
private void ShoulderDrive(Arm a, int side, float dt)
{
Drive d = a.Shoulder;
Transform3D chest = Chest.GlobalTransform.Orthonormalized();
Vector3 desired = chest * (d.AnchorA + _girdle[side]);
Vector3 err = desired - a.Upper.GlobalTransform * d.AnchorB;
Quaternion frameA = chest.Basis.GetRotationQuaternion() * d.FrameA;
SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp));
Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB);
Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse()
* (_targets[a.Upper].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameB);
Vector3 ff = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
d.PrevTarget = target;
d.HasPrev = true;
SetAngularVelocity(d.Joint, (ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin));
}
// Force-capped pull of the hand onto the controller, reacting into the chest (arm muscles).
private void HandGuide(Arm a, Transform3D target, Vector3 targetVelocity)
{
Transform3D chest = Chest.GlobalTransform;
Vector3 anchor = chest * a.Guide.AnchorA;
Vector3 anchorVelocity = Chest.LinearVelocity + Chest.AngularVelocity.Cross(anchor - Chest.GlobalPosition);
Vector3 desired = (targetVelocity + (target.Origin - a.Hand.GlobalPosition) * HandGuideKp).LimitLength(HandGuideMaxSpeed);
Quaternion frameA = Rot(Chest) * a.Guide.FrameA;
SetLinearVelocity(a.Guide.Joint, frameA.Inverse() * (desired - anchorVelocity));
// Angular: hand rotation onto the controller rotation, as a relative spin vs the chest.
Vector3 err = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand));
Vector3 wWorld = (err * HandGuideAngularKp).LimitLength(MuscleMaxSpin * 2f) - Chest.AngularVelocity;
SetAngularVelocity(a.Guide.Joint, frameA.Inverse() * wWorld);
}
// "Leg strength": a stiff, mass-scaled support holding the pelvis at its pose height above the
// ground and carrying it after the head horizontally (friction-limited), plus an upright/yaw
// balance motor (world anchor → pelvis). No ground → no support (falls); balance weakens in the air.
private void GroundControl(Transform3D target, float floorY, float dt)
{
Vector3 pos = Pelvis.GlobalPosition;
float hipAboveFloor = target.Origin.Y - floorY;
var query = PhysicsRayQueryParameters3D.Create(pos, pos + Vector3.Down * (hipAboveFloor + GroundProbeExtra + 0.5f), WorldMask);
Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query);
Grounded = false;
if (hit.Count > 0)
{
Vector3 point = (Vector3)hit["position"];
Vector3 normal = (Vector3)hit["normal"];
if (pos.Y - point.Y < hipAboveFloor + GroundProbeExtra && normal.Y > 0.6f)
{
Grounded = true;
GroundY = point.Y;
}
}
Vector3 targetVelocity = _havePelvisPrev && dt > 0f ? (target.Origin - _pelvisTargetPrev) / dt : Vector3.Zero;
_pelvisTargetPrev = target.Origin;
_havePelvisPrev = true;
if (targetVelocity.Length() > 10f)
targetVelocity = Vector3.Zero;
float weight = BodyMass * 9.8f;
if (Grounded)
{
// Pose heights are relative to the tracking floor; stand on the ground actually under us.
Vector3 goal = target.Origin + Vector3.Up * (GroundY - floorY);
float w = Mathf.Tau * GroundFrequency;
Vector3 accel = (goal - pos) * (w * w) + (targetVelocity - Pelvis.LinearVelocity) * (2f * GroundDamping * w);
Vector3 force = accel * BodyMass;
float support = Mathf.Clamp(force.Y + weight, 0f, LegStrength * weight);
Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(GroundFriction * support);
Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support);
}
Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(Pelvis));
Vector3 spin = (rotErr * BalanceKp).LimitLength(MuscleMaxSpin);
SetAngularVelocity(_balance.Joint, _balance.FrameA.Inverse() * spin); // anchor = world frame
float cap = BalanceTorque * (Grounded ? 1f : AirBalanceShare);
if (cap != _balanceCap)
{
_balanceCap = cap;
_balance.Joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
_balance.Joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
_balance.Joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
}
}
private Drive _balance;
private float _balanceCap;
// ---------------------------------------------------------------- play space
// How the driver should move its play-space origin after the last physics step (Bonelab-style
// push-back): a sphere swept from the head body's eye to the camera — if geometry is in the way
// the camera is somewhere the head can't follow (a wall, a bar, a table it walked into), so the
// view is put back at the last free point. With nothing in between, the head may lag or be
// held up to FreeSlack (leaning over a table with the hips blocked) before the view follows.
// Plus: the floor follows the ground under the body; airborne, the view rides the body (falls).
public Vector3 OriginCorrection(Vector3 camera, float floorY, float dt)
{
if (!Built)
return Vector3.Zero;
Vector3 eye = Eye;
Vector3 lead = camera - eye;
Vector3 correction = Vector3.Zero;
if (lead.LengthSquared() > 1e-8f)
{
_cameraQuery ??= new PhysicsShapeQueryParameters3D { Shape = new SphereShape3D { Radius = CameraRadius }, CollisionMask = WorldMask };
_cameraQuery.Transform = new Transform3D(Basis.Identity, eye);
_cameraQuery.Motion = lead;
float[] fractions = GetWorld3D().DirectSpaceState.CastMotion(_cameraQuery);
float safe = fractions.Length > 0 ? fractions[0] : 1f;
if (safe < 1f)
correction = -lead * (1f - safe);
else if (lead.Length() > FreeSlack)
correction = -lead * (1f - FreeSlack / lead.Length());
}
if (Grounded)
{
correction.Y = 0f;
correction.Y += (GroundY - floorY) * (1f - Mathf.Exp(-dt * GroundFollowRate));
}
return correction;
}
private PhysicsShapeQueryParameters3D _cameraQuery;
public void TeleportBy(Transform3D delta)
{
if (!Built)
return;
foreach (RigidBody3D b in _bodies)
{
b.GlobalTransform = delta * b.GlobalTransform;
b.LinearVelocity = delta.Basis * b.LinearVelocity;
b.AngularVelocity = delta.Basis * b.AngularVelocity;
}
_pelvisTargetPrev = delta * _pelvisTargetPrev;
}
public void SnapToPose(TrueBodySolver s, Transform3D left, Transform3D right)
{
ComputeTargets(s, left, right);
foreach (RigidBody3D b in _bodies)
{
b.GlobalTransform = Target(b);
b.LinearVelocity = Vector3.Zero;
b.AngularVelocity = Vector3.Zero;
}
// The hands sit at the wrist end of the solved arm, not at a possibly unreachable controller.
for (int side = 0; side < 2; side++)
{
Arm a = _arms[side];
Basis hb = _targets[a.Hand].Basis;
a.Hand.GlobalTransform = new Transform3D(hb, s.Wrist[side] - hb * new Vector3(0f, 0f, WristOffset));
}
_havePelvisPrev = false;
}
// Collision exceptions between a held prop and that arm's segments (its hand is excluded by
// the grab weld itself) — a forearm wrapped around a held object must not fight it.
public void SetHeld(int side, RigidBody3D prop)
{
if (!Built)
return;
Arm a = _arms[side];
if (a.Held != null && IsInstanceValid(a.Held) && a.Held != _arms[1 - side].Held)
{
a.Upper.RemoveCollisionExceptionWith(a.Held);
a.Fore.RemoveCollisionExceptionWith(a.Held);
}
a.Held = prop;
if (prop != null)
{
a.Upper.AddCollisionExceptionWith(prop);
a.Fore.AddCollisionExceptionWith(prop);
}
}
}
scripts/PlayerRagdoll.cs.uid
+1
View File
@@ -0,0 +1 @@
uid://dvwdt7eb4nsm2
scripts/PlayerRig.cs
+205
View File
@@ -0,0 +1,205 @@
using Godot;
// In-game driver of the ragdoll player (body step 2): XR input → True Body → PlayerRagdoll, plus
// Boneworks-style stick locomotion and the play-space reconciliation. Runs everything in one
// physics tick, in this order:
// 1. discrete origin jumps made by other code (spawn, teleport markers) carry the body along;
// 2. stick: left = move (head-relative), right X = turn (snap by default, SmoothTurn for smooth);
// 3. the ragdoll's OriginCorrection (push-back at walls, floor follows the ground, falls);
// 4. True Body solve from the corrected camera/controllers, then the ragdoll's muscles.
// Waits for the calibrated BodyProfile like everything else; rebuilds on recalibration.
public partial class PlayerRig : Node
{
public const float MoveSpeed = 2.2f; // m/s at full stick (Boneworks-ish walk)
public const float MoveAcceleration = 8f; // m/s²
public const float StickDeadzone = 0.2f;
public static bool SmoothTurn = false;
public const float SnapTurnDegrees = 30f;
public const float SmoothTurnSpeed = 2.5f; // rad/s
public const float SnapTurnThreshold = 0.7f;
public const float SnapTurnRearm = 0.4f;
public const float JumpDistance = 0.5f; // an origin move this big in one tick = teleport, not motion
public const float JumpAngle = 0.1f; // rad
private const float GhostLagDistance = 0.1f;
private static PlayerRig _instance;
private XROrigin3D _origin;
private XRCamera3D _camera;
private XRController3D _left, _right;
private TrueBody _trueBody;
private PlayerRagdoll _ragdoll;
private Transform3D _lastOrigin;
private bool _haveLastOrigin;
private Vector3 _moveVelocity;
private bool _snapArmed = true;
private readonly Vector3[] _ctrlPrev = new Vector3[2];
private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
private bool _rebuild = true;
public PlayerRagdoll Ragdoll => _ragdoll;
public static RigidBody3D GetHandBody(XRController3D controller)
{
if (_instance == null || !_instance._ragdoll.Built)
return null;
return _instance._ragdoll.Hand(controller == _instance._left ? 0 : 1);
}
public static void SetHeld(XRController3D controller, RigidBody3D prop)
{
if (_instance != null)
_instance._ragdoll.SetHeld(controller == _instance._left ? 0 : 1, prop);
}
public override void _Ready()
{
_instance = this;
_origin = GetNode<XROrigin3D>("../XROrigin3D");
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
_trueBody = GetNodeOrNull<TrueBody>("../TrueBody");
if (_trueBody != null)
{
_trueBody.ExternallySolved = true;
TrueBody.ShowDebug = false;
}
_ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
AddChild(_ragdoll);
BodyProfile.Changed += _ => _rebuild = true;
for (int i = 0; i < 2; i++)
{
_ghosts[i] = new MeshInstance3D
{
Mesh = new BoxMesh { Size = PlayerRagdoll.HandSize },
Visible = false,
CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
};
_ghosts[i].SetSurfaceOverrideMaterial(0, new StandardMaterial3D
{
AlbedoColor = new Color(1f, 1f, 1f, 0.3f),
Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
});
AddChild(_ghosts[i]);
}
}
private TrueBodySolver Solver => _trueBody != null ? _trueBody.Solver : _ownSolver;
private readonly TrueBodySolver _ownSolver = new();
public override void _PhysicsProcess(double delta)
{
float dt = (float)delta;
BodyProfile profile = BodyProfile.Current;
if (profile == null)
return;
// 1. Someone else moved the play space discretely: bring the body along.
Transform3D origin = _origin.GlobalTransform;
if (_haveLastOrigin && _ragdoll.Built && IsJump(_lastOrigin, origin))
Carry(origin * _lastOrigin.AffineInverse());
if (_ragdoll.Built && !_rebuild)
{
// 2. Stick locomotion, 3. reconciliation.
Locomote(dt);
Vector3 correction = _ragdoll.OriginCorrection(_camera.GlobalPosition, _origin.GlobalPosition.Y, dt);
if (correction != Vector3.Zero)
_origin.GlobalPosition += correction;
}
// 4. Pose + muscles.
Transform3D head = _camera.GlobalTransform;
Transform3D left = _left.GlobalTransform, right = _right.GlobalTransform;
Solver.Solve(profile, head, left, right, _origin.GlobalPosition.Y, dt);
if (_rebuild || !_ragdoll.Built)
{
_ragdoll.Build(profile, Solver, left, right);
_rebuild = false;
_ctrlPrev[0] = left.Origin;
_ctrlPrev[1] = right.Origin;
}
Vector3 lv = (left.Origin - _ctrlPrev[0]) / dt, rv = (right.Origin - _ctrlPrev[1]) / dt;
_ctrlPrev[0] = left.Origin;
_ctrlPrev[1] = right.Origin;
_ragdoll.Step(Solver, left, right, lv, rv, _origin.GlobalPosition.Y, dt);
_lastOrigin = _origin.GlobalTransform;
_haveLastOrigin = true;
}
public override void _Process(double delta)
{
for (int i = 0; i < 2; i++)
{
XRController3D c = i == 0 ? _left : _right;
RigidBody3D hand = _ragdoll.Built ? _ragdoll.Hand(i) : null;
bool lagging = hand != null && hand.GlobalPosition.DistanceTo(c.GlobalPosition) > GhostLagDistance;
_ghosts[i].Visible = lagging;
if (lagging)
_ghosts[i].GlobalTransform = c.GlobalTransform;
}
}
private static bool IsJump(Transform3D a, Transform3D b) =>
a.Origin.DistanceTo(b.Origin) > JumpDistance ||
BodyMath.RotationError(b.Basis.GetRotationQuaternion(), a.Basis.GetRotationQuaternion()).Length() > JumpAngle;
private void Carry(Transform3D delta)
{
_ragdoll.TeleportBy(delta);
Solver.Shift(delta);
_ctrlPrev[0] = delta * _ctrlPrev[0];
_ctrlPrev[1] = delta * _ctrlPrev[1];
}
private void Locomote(float dt)
{
// A stick whose hand points at the menu scrolls it instead.
Vector2 move = WorldPanel.IsPointing(_left) ? Vector2.Zero : _left.GetVector2("primary");
Vector2 turn = WorldPanel.IsPointing(_right) ? Vector2.Zero : _right.GetVector2("primary");
// Move: head-relative on the ground plane, acceleration-limited. The body follows the
// camera physically, so a wall stops it and the push-back stops the view.
Vector3 desired = Vector3.Zero;
if (move.Length() > StickDeadzone)
{
Vector3 fwd = -_camera.GlobalTransform.Basis.Z;
fwd.Y = 0f;
fwd = fwd.LengthSquared() > 1e-6f ? fwd.Normalized() : Vector3.Forward;
Vector3 rightDir = fwd.Cross(Vector3.Up);
float mag = Mathf.Clamp((move.Length() - StickDeadzone) / (1f - StickDeadzone), 0f, 1f);
desired = (fwd * move.Y + rightDir * move.X).Normalized() * (mag * MoveSpeed);
}
if (!_ragdoll.Grounded)
desired = _moveVelocity; // no air control
_moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt);
if (_moveVelocity != Vector3.Zero)
_origin.GlobalPosition += _moveVelocity * dt;
// Turn about the head.
float angle = 0f;
if (SmoothTurn)
{
if (Mathf.Abs(turn.X) > StickDeadzone)
angle = -turn.X * SmoothTurnSpeed * dt;
}
else if (_snapArmed && Mathf.Abs(turn.X) > SnapTurnThreshold)
{
angle = -Mathf.Sign(turn.X) * Mathf.DegToRad(SnapTurnDegrees);
_snapArmed = false;
}
else if (Mathf.Abs(turn.X) < SnapTurnRearm)
_snapArmed = true;
if (angle != 0f)
{
Vector3 pivot = _camera.GlobalPosition;
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, angle), Vector3.Zero)
* new Transform3D(Basis.Identity, -pivot);
_origin.GlobalTransform = rot * _origin.GlobalTransform;
if (!SmoothTurn)
Carry(rot);
}
}
}
scripts/PlayerRig.cs.uid
+1
View File
@@ -0,0 +1 @@
uid://n62b8ehiuaeb
scripts/PropGrabber.cs
+2
View File
@@ -69,6 +69,7 @@ public partial class PropGrabber : Node
_joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias); _joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias);
_localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot; _localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot;
_arms?.SetHeld(hand, prop); _arms?.SetHeld(hand, prop);
PlayerRig.SetHeld(controller, prop);
GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})"); GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})");
} }
} }
@@ -186,6 +187,7 @@ public partial class PropGrabber : Node
private void DestroyJoint(int hand) private void DestroyJoint(int hand)
{ {
_arms?.SetHeld(hand, null); _arms?.SetHeld(hand, null);
PlayerRig.SetHeld(hand == 0 ? _left : _right, null);
if (_joints[hand] != null && IsInstanceValid(_joints[hand])) if (_joints[hand] != null && IsInstanceValid(_joints[hand]))
_joints[hand].QueueFree(); _joints[hand].QueueFree();
_joints[hand] = null; _joints[hand] = null;
scripts/RagdollSpike.cs
+476
View File
@@ -0,0 +1,476 @@
using System;
using System.Collections.Generic;
using Godot;
// Headless regression for body step 2 — the muscle-driven ragdoll player (PlayerRagdoll) driven
// exactly like PlayerRig drives it, from a simulated play space (origin + head/controller poses).
// godot-mono --headless --fixed-fps 90 --path . scenes/ragdoll_spike.tscn
// Must print "RAGDOLL: overall PASS".
public partial class RagdollSpike : Node3D
{
private const float EyeHeight = 1.65f;
private readonly List<(string Name, Action Setup, Action<float> Tick, float Duration, Func<bool> Finish)> _phases = new();
private int _phase = -1;
private float _t;
private bool _allOk = true;
private Node3D _stage;
private PlayerRagdoll _ragdoll;
private readonly TrueBodySolver _solver = new();
private BodyProfile _profile;
// Simulated play space.
private Transform3D _origin = Transform3D.Identity;
private Transform3D _headLocal, _leftLocal, _rightLocal;
private Vector3 _stickVelocity;
private Vector3 _prevL, _prevR;
private bool _needBuild;
// Measurements (reset per phase).
private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
private bool _finite;
private int _snapsAtStart;
private float _measureFrom;
private Transform3D Camera => _origin * _headLocal;
private Transform3D Left => _origin * _leftLocal;
private Transform3D Right => _origin * _rightLocal;
public override void _Ready()
{
_profile = BodyProfile.Estimate(EyeHeight);
AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29));
_ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
AddChild(_ragdoll);
_phases.Add(("hinge sign", SetupHinge, TickHinge, 0.3f, FinishHinge));
_phases.Add(("stands still, head on the camera, hands on the controllers", Rest, _ => { }, 4f, FinishStand));
_phases.Add(("reaches forward, out and overhead", Rest, TickReach, 4.5f, FinishReach));
_phases.Add(("fast arm swings stay attached", Rest, TickSwing, 3f, FinishSwing));
_phases.Add(("stick-walking into a wall is stopped (view pushed back)", SetupWall, _ => _stickVelocity = new Vector3(0f, 0f, -2f), 3f, FinishWall));
_phases.Add(("room-scale walking into a wall is stopped", SetupWall, TickRoomWalk, 3f, FinishWall));
_phases.Add(("standing walk into a low bar is blocked", SetupBar, _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishBarBlocked));
_phases.Add(("crouched walk passes under the low bar", SetupBar, TickCrouchWalk, 4f, FinishBarPassed));
_phases.Add(("leans over a table without being pushed back", SetupTable, TickLean, 3f, FinishLean));
_phases.Add(("pushes a 10 kg crate", SetupCrate, TickPush, 3f, FinishCrate));
_phases.Add(("12 kg held close to the body", () => SetupWeight(close: true), _ => { }, 3f, () => FinishWeight(close: true)));
_phases.Add(("12 kg held at arm's length sags more", () => SetupWeight(close: false), _ => { }, 3f, () => FinishWeight(close: false)));
_phases.Add(("teleport + snap turn carry the body", Rest, TickTeleport, 2f, FinishTeleport));
_phases.Add(("random motion soak", Rest, TickRandom, 8f, FinishSoak));
Next();
}
// ---------------------------------------------------------------- harness
public override void _PhysicsProcess(double delta)
{
if (_phase >= _phases.Count)
return;
float dt = (float)delta;
_t += dt;
_phases[_phase].Tick(dt);
if (_phase > 0)
Drive(dt);
if (_t >= _phases[_phase].Duration)
{
bool ok = _phases[_phase].Finish();
_allOk &= ok;
GD.Print($"RAGDOLL: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}");
Next();
}
}
private void Next()
{
_stage?.QueueFree();
_stage = new Node3D();
AddChild(_stage);
_t = 0f;
_phase++;
if (_phase >= _phases.Count)
{
GD.Print($"RAGDOLL: overall {(_allOk ? "PASS — muscle ragdoll player functional" : "CHECK — inspect failures above")}");
GetTree().Quit();
return;
}
_maxEyeErr = _maxHandErr = _endHandErr = _maxSpeed = _maxElbowErr = 0f;
_minCamZ = _minEyeZ = float.MaxValue;
_finite = true;
_measureFrom = 1f;
_stickVelocity = Vector3.Zero;
_phases[_phase].Setup();
_snapsAtStart = _ragdoll.Snaps;
}
// Exactly PlayerRig's tick order.
private void Drive(float dt)
{
_origin.Origin += _stickVelocity * dt;
if (_ragdoll.Built && !_needBuild)
_origin.Origin += _ragdoll.OriginCorrection(Camera.Origin, _origin.Origin.Y, dt);
_solver.Solve(_profile, Camera, Left, Right, _origin.Origin.Y, dt);
if (_needBuild)
{
_ragdoll.Build(_profile, _solver, Left, Right);
_needBuild = false;
_prevL = Left.Origin;
_prevR = Right.Origin;
}
Vector3 lv = (Left.Origin - _prevL) / dt, rv = (Right.Origin - _prevR) / dt;
_prevL = Left.Origin;
_prevR = Right.Origin;
_ragdoll.Step(_solver, Left, Right, lv, rv, _origin.Origin.Y, dt);
Measure();
}
private void Measure()
{
foreach (RigidBody3D b in _ragdoll.Bodies)
_finite &= BodyMath.IsFinite(b);
_minCamZ = Mathf.Min(_minCamZ, Camera.Origin.Z);
_minEyeZ = Mathf.Min(_minEyeZ, _ragdoll.Eye.Z);
if (_t < _measureFrom)
return;
_maxEyeErr = Mathf.Max(_maxEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
float handErr = Mathf.Max(_ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin));
_maxHandErr = Mathf.Max(_maxHandErr, handErr);
_endHandErr = handErr;
float speed = 0f;
foreach (RigidBody3D b in _ragdoll.Bodies)
speed = Mathf.Max(speed, b.LinearVelocity.Length());
_maxSpeed = speed;
for (int side = 0; side < 2; side++)
{
Vector3 up = _solver.Elbow[side] - _solver.Shoulder[side], fore = _solver.Wrist[side] - _solver.Elbow[side];
float target = up.AngleTo(fore);
_maxElbowErr = Mathf.Max(_maxElbowErr, Mathf.Abs(target - _ragdoll.ElbowFlex(side)));
}
}
private int NewSnaps => _ragdoll.Snaps - _snapsAtStart;
private static StaticBody3D MakeStatic(Vector3 size, Vector3 pos, uint mask = 0)
{
StaticBody3D body = new() { Position = pos, CollisionMask = mask };
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
return body;
}
private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos)
{
RigidBody3D body = new() { Mass = mass, Position = pos, CollisionMask = 29 };
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
_stage.AddChild(body);
return body;
}
// Standing, looking ahead, hands in front at waist height (elbows ~90°), controllers pointing forward.
private void Rest()
{
_origin = Transform3D.Identity;
_headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f, -0.3f));
_solver.Reset();
_needBuild = true;
}
// ---------------------------------------------------------------- hinge sign
private RigidBody3D _hingeBody;
private HingeJoint3D _hinge;
private Basis _hingeStart;
private void SetupHinge()
{
StaticBody3D anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0f, 5f, 0f));
_stage.AddChild(anchor);
_hingeBody = new RigidBody3D { Position = new Vector3(0f, 5f, 0f), GravityScale = 0f };
_hingeBody.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.4f, 0.1f, 0.1f) } });
_stage.AddChild(_hingeBody);
_hinge = new HingeJoint3D { Position = new Vector3(0f, 5f, 0f), NodeA = anchor.GetPath(), NodeB = _hingeBody.GetPath() };
_hinge.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
_hinge.SetParam(HingeJoint3D.Param.MotorTargetVelocity, 1f);
_hinge.SetParam(HingeJoint3D.Param.MotorMaxImpulse, 50f);
_stage.AddChild(_hinge);
_hingeStart = _hingeBody.GlobalTransform.Basis;
}
private void TickHinge(float dt) { }
private bool FinishHinge()
{
// Joint +Z = world +Z here. Positive motor velocity rotated the body about ±Z?
Vector3 x = _hingeBody.GlobalTransform.Basis.X;
float about = _hingeStart.X.Cross(x).Z;
PlayerRagdoll.HingeSign = about >= 0f ? 1f : -1f;
GD.Print($"RAGDOLL: +motor velocity turns the child about {(about >= 0 ? "+" : "-")}Z → HingeSign {PlayerRagdoll.HingeSign}");
return Mathf.Abs(about) > 0.05f;
}
// ---------------------------------------------------------------- phases
private bool FinishStand()
{
GD.Print($"RAGDOLL: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3} m, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, end speed {_maxSpeed:F3} m/s, grounded {_ragdoll.Grounded}, snaps {NewSnaps}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F3}");
return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && _maxSpeed < 0.05f && _maxElbowErr < Mathf.DegToRad(8f);
}
private readonly List<string> _reachLog = new();
private bool _reachOk = true;
private void TickReach(float dt)
{
_measureFrom = float.MaxValue;
float reach = _profile.ArmLength;
float sh = EyeHeight - _profile.ShoulderDrop;
(Vector3 l, Vector3 r)[] poses =
{
(new Vector3(-0.2f, sh, -reach - 0.05f), new Vector3(0.2f, sh, -reach - 0.05f)), // forward, straight
(new Vector3(-_profile.ShoulderHalfWidth - reach, sh, 0f), new Vector3(_profile.ShoulderHalfWidth + reach, sh, 0f)), // T
(new Vector3(-0.2f, sh + reach, -0.05f), new Vector3(0.2f, sh + reach, -0.05f)), // overhead
};
int i = Mathf.Min((int)(_t / 1.5f), poses.Length - 1);
float local = _t - i * 1.5f;
// Controllers point along the arm, as a real hand does at full reach.
Vector3 shL = new(-_profile.ShoulderHalfWidth, sh, 0.06f), shR = new(_profile.ShoulderHalfWidth, sh, 0.06f);
_leftLocal = new Transform3D(Aim(poses[i].l - shL), poses[i].l);
_rightLocal = new Transform3D(Aim(poses[i].r - shR), poses[i].r);
if (local > 1.4f && local - dt <= 1.4f)
{
// Unreachable by up to the girdle travel: the hand stops at the arm's end, not inside the body.
float errL = _ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), errR = _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin);
float e = Mathf.Max(errL, errR);
_reachLog.Add($"pose {i}: hand error {e:F3} m");
_reachOk &= e < 0.06f;
}
}
// Controller basis whose aim (−Z) points along `dir`, thumb up where possible.
private static Basis Aim(Vector3 dir)
{
Vector3 z = -dir.Normalized();
Vector3 up = Mathf.Abs(z.Y) > 0.9f ? Vector3.Back : Vector3.Up;
Vector3 x = up.Cross(z).Normalized();
return new Basis(x, z.Cross(x), z);
}
private bool FinishReach()
{
GD.Print($"RAGDOLL: {string.Join(", ", _reachLog)}; snaps {NewSnaps}");
bool ok = _finite && NewSnaps == 0 && _reachOk && _reachLog.Count == 3;
_reachLog.Clear();
_reachOk = true;
return ok;
}
private void TickSwing(float dt)
{
float a = Mathf.Sin(_t * Mathf.Tau * 1.5f);
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.25f, 1.15f + 0.25f * a, -0.35f - 0.15f * a));
_rightLocal = new Transform3D(new Basis(Vector3.Right, 0.8f * a), new Vector3(0.25f + 0.2f * a, 1.15f - 0.25f * a, -0.35f));
}
private bool FinishSwing()
{
GD.Print($"RAGDOLL: hand lag max {_maxHandErr:F3} m (limit 0.15), eye error max {_maxEyeErr:F3}, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _maxHandErr < 0.15f && _maxEyeErr < 0.08f;
}
private const float WallFace = -1.0f;
private void SetupWall()
{
Rest();
_stage.AddChild(MakeStatic(new Vector3(4f, 3f, 0.4f), new Vector3(0f, 1.5f, WallFace - 0.2f), 29));
}
private void TickRoomWalk(float dt)
{
Vector3 step = new(0f, 0f, -1f * dt);
_headLocal.Origin += step;
_leftLocal.Origin += step;
_rightLocal.Origin += step;
}
private bool FinishWall()
{
GD.Print($"RAGDOLL: wall face z {WallFace}: camera min z {_minCamZ:F3}, eye min z {_minEyeZ:F3}, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _minCamZ > WallFace - 0.02f && _minEyeZ > WallFace - 0.02f && _minCamZ < WallFace + 0.5f;
}
private const float BarZ = -0.8f;
private void SetupBar()
{
Rest();
// A beam across the path whose underside is at 1.2 m.
_stage.AddChild(MakeStatic(new Vector3(4f, 0.1f, 0.1f), new Vector3(0f, 1.25f, BarZ), 29));
}
private bool FinishBarBlocked()
{
// The chest hits the beam; the head may lean over it, but the view can't run off ahead of the body.
GD.Print($"RAGDOLL: bar at z {BarZ}: eye min z {_minEyeZ:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _minEyeZ > BarZ - 0.25f && _minCamZ > _minEyeZ - PlayerRagdoll.FreeSlack - 0.02f;
}
private void TickCrouchWalk(float dt)
{
// Crouch over the first second (eyes to 1.0 m, looking slightly down), then walk forward.
float k = Mathf.Min(_t / 1f, 1f);
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.0f, k), 0f));
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
_stickVelocity = _t > 1.2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
}
private bool FinishBarPassed()
{
GD.Print($"RAGDOLL: crouched: camera min z {_minCamZ:F3} (bar at {BarZ}), eye {_ragdoll.Eye.Y:F2} m, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _minCamZ < BarZ - 0.6f;
}
private void SetupTable()
{
Rest();
_stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, -0.85f), 29));
}
private void TickLean(float dt)
{
float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.6f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.3f, k), Mathf.Lerp(0f, -0.4f, k)));
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
_measureFrom = 2f;
}
private bool FinishLean()
{
GD.Print($"RAGDOLL: eye error after the lean max {_maxEyeErr:F3} m (limit 0.08), hand error {_endHandErr:F3}, origin drift {_origin.Origin.Length():F3}, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _origin.Origin.Length() < 0.05f;
}
private RigidBody3D _crate;
private Vector3 _crateStart;
private void SetupCrate()
{
Rest();
_crate = MakeBox(new Vector3(0.4f, 1.4f, 0.4f), 10f, new Vector3(0f, 0.7f, -0.55f)); // within reach at chest height
_crateStart = _crate.Position;
}
private void TickPush(float dt)
{
float k = Mathf.Clamp((_t - 0.5f) / 2f, 0f, 1f);
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
}
private bool FinishCrate()
{
float moved = _crateStart.Z - _crate.GlobalPosition.Z;
GD.Print($"RAGDOLL: crate pushed {moved:F3} m (front face now z {_crate.GlobalPosition.Z + 0.2f:F3}), hands z {_ragdoll.Hand(0).GlobalPosition.Z:F3}/{_ragdoll.Hand(1).GlobalPosition.Z:F3} y {_ragdoll.Hand(0).GlobalPosition.Y:F3}, elbows {Mathf.RadToDeg(_ragdoll.ElbowFlex(0)):F0}°, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && moved > 0.15f;
}
private RigidBody3D _weight;
private float _closeSag = -1f;
private void SetupWeight(bool close)
{
Rest();
float sh = EyeHeight - _profile.ShoulderDrop;
_rightLocal = close
? new Transform3D(Basis.Identity, new Vector3(0.18f, sh - 0.1f, -0.25f))
: new Transform3D(Basis.Identity, new Vector3(_profile.ShoulderHalfWidth, sh, -_profile.ArmLength * 0.95f));
_measureFrom = 2f;
// Build now so the weight can be welded to the placed hand.
Drive(1f / 90f);
RigidBody3D hand = _ragdoll.Hand(1);
Vector3 at = hand.GlobalTransform * new Vector3(0f, 0f, -0.12f); // just beyond the fingers
_weight = MakeBox(new Vector3(0.12f, 0.12f, 0.12f), 12f, at);
_weight.GlobalPosition = at;
_ragdoll.SetHeld(1, _weight);
ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _weight.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
_stage.AddChild(weld);
}
private bool FinishWeight(bool close)
{
float sag = Right.Origin.Y - _ragdoll.Hand(1).GlobalPosition.Y;
GD.Print($"RAGDOLL: right hand sag {sag:F3} m under 12 kg ({(close ? "close" : "far")}), snaps {NewSnaps}");
if (close)
{
_closeSag = sag;
return _finite && NewSnaps == 0 && sag < 0.08f;
}
return _finite && NewSnaps == 0 && sag > _closeSag + 0.03f;
}
private bool _teleported, _turned;
private float _teleportEyeErr;
private void TickTeleport(float dt)
{
_measureFrom = 1.2f;
if (!_teleported && _t > 0.5f)
{
Transform3D before = _origin;
_origin = new Transform3D(new Basis(Vector3.Up, Mathf.Pi / 2f), new Vector3(5f, 0f, 3f));
Transform3D delta = _origin * before.AffineInverse();
_ragdoll.TeleportBy(delta);
_solver.Shift(delta);
_prevL = delta * _prevL;
_prevR = delta * _prevR;
_teleported = true;
}
if (!_turned && _t > 0.9f)
{
Vector3 pivot = Camera.Origin;
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, Mathf.DegToRad(30f)), Vector3.Zero) * new Transform3D(Basis.Identity, -pivot);
_origin = rot * _origin;
_ragdoll.TeleportBy(rot);
_solver.Shift(rot);
_prevL = rot * _prevL;
_prevR = rot * _prevR;
_turned = true;
}
}
private bool FinishTeleport()
{
GD.Print($"RAGDOLL: after teleport + snap turn: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3}, snaps {NewSnaps}");
_teleported = _turned = false;
return _finite && NewSnaps == 0 && _maxEyeErr < 0.05f && _maxHandErr < 0.06f;
}
private readonly RandomNumberGenerator _rng = new() { Seed = 11 };
private Vector3 _ra, _rb, _rc;
private void TickRandom(float dt)
{
if (_t < dt * 1.5f)
{
_ra = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
_rb = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
_rc = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
}
Vector3 W(Vector3 ph, float f) => new(Mathf.Sin(_t * f + ph.X), Mathf.Sin(_t * f * 1.3f + ph.Y), Mathf.Sin(_t * f * 0.7f + ph.Z));
Vector3 h = W(_ra, 1.1f);
_headLocal = new Transform3D(Basis.FromEuler(new Vector3(0.4f * h.X, 0.9f * h.Y, 0.2f * h.Z)), new Vector3(0.1f * h.Z, EyeHeight - 0.15f + 0.1f * h.X, 0.1f * h.Y));
float sh = EyeHeight - _profile.ShoulderDrop;
Vector3 l = W(_rb, 2.3f), r = W(_rc, 2.0f);
_leftLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * l.Y, 0.8f * l.Z, 1.0f * l.X)), new Vector3(-0.25f + 0.15f * l.X, sh - 0.25f + 0.2f * l.Y, -0.3f + 0.15f * l.Z));
_rightLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * r.Y, 0.8f * r.Z, 1.0f * r.X)), new Vector3(0.25f + 0.15f * r.X, sh - 0.25f + 0.2f * r.Y, -0.3f + 0.15f * r.Z));
}
private bool FinishSoak()
{
GD.Print($"RAGDOLL: random motion: eye error max {_maxEyeErr:F3}, hand error max {_maxHandErr:F3}, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, snaps {NewSnaps}");
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _maxHandErr < 0.12f;
}
}
scripts/RagdollSpike.cs.uid
+1
View File
@@ -0,0 +1 @@
uid://bpdb6t6xgwt24
scripts/TrueBody.cs
+25 -1
View File
@@ -26,8 +26,13 @@ public sealed class TrueBodySolver
public const float WristOffset = 0.06f; public const float WristOffset = 0.06f;
public Vector3 Eye, NeckPivot, NeckBase, UpperChest, Pelvis; public Vector3 Eye, NeckPivot, NeckBase, UpperChest, Pelvis;
public Vector3 PelvisTarget; // un-lagged pelvis (the ragdoll's ground control adds its own lag)
public Basis ChestBasis = Basis.Identity; public Basis ChestBasis = Basis.Identity;
public Basis PelvisBasis = Basis.Identity; public Basis PelvisBasis = Basis.Identity;
public Basis YawBasis = Basis.Identity;
public Basis HeadBasis = Basis.Identity;
public float Lean { get; private set; }
public readonly Vector3[] Pole = new Vector3[2]; // per arm, see ArmSolver.Pole
public readonly Vector3[] Shoulder = new Vector3[2]; public readonly Vector3[] Shoulder = new Vector3[2];
public readonly Vector3[] Elbow = new Vector3[2]; public readonly Vector3[] Elbow = new Vector3[2];
public readonly Vector3[] Wrist = new Vector3[2]; public readonly Vector3[] Wrist = new Vector3[2];
@@ -40,6 +45,18 @@ public sealed class TrueBodySolver
private Vector3 _pelvisVelocity; private Vector3 _pelvisVelocity;
private float _yaw; private float _yaw;
// A discrete move of the whole player (teleport, snap turn): carry the lagging state along
// instead of letting it swing across the gap.
public void Shift(Transform3D delta)
{
if (!Initialized)
return;
Pelvis = delta * Pelvis;
_pelvisVelocity = delta.Basis * _pelvisVelocity;
Vector3 f = delta.Basis * new Basis(Vector3.Up, _yaw) * Vector3.Forward;
_yaw = Mathf.Atan2(-f.X, -f.Z);
}
public void Reset() public void Reset()
{ {
Initialized = false; Initialized = false;
@@ -52,6 +69,7 @@ public sealed class TrueBodySolver
{ {
Eye = head.Origin; Eye = head.Origin;
Basis headBasis = head.Basis.Orthonormalized(); Basis headBasis = head.Basis.Orthonormalized();
HeadBasis = headBasis;
NeckPivot = Eye + headBasis * EyeToNeckPivot; NeckPivot = Eye + headBasis * EyeToNeckPivot;
WristTarget[0] = left * new Vector3(0f, 0f, WristOffset); WristTarget[0] = left * new Vector3(0f, 0f, WristOffset);
WristTarget[1] = right * new Vector3(0f, 0f, WristOffset); WristTarget[1] = right * new Vector3(0f, 0f, WristOffset);
@@ -73,6 +91,7 @@ public sealed class TrueBodySolver
_yaw = hmdYaw; _yaw = hmdYaw;
_yaw = Mathf.LerpAngle(_yaw, targetYaw, 1f - Mathf.Exp(-dt * YawSmoothRate)); _yaw = Mathf.LerpAngle(_yaw, targetYaw, 1f - Mathf.Exp(-dt * YawSmoothRate));
Basis yawBasis = new Basis(Vector3.Up, _yaw); Basis yawBasis = new Basis(Vector3.Up, _yaw);
YawBasis = yawBasis;
// Crouch lean: measured at the NECK PIVOT (nodding moves the eyes ~10 cm but not the pivot), // Crouch lean: measured at the NECK PIVOT (nodding moves the eyes ~10 cm but not the pivot),
// below its standing height the chest pitches forward; looking down adds a little. // below its standing height the chest pitches forward; looking down adds a little.
@@ -81,6 +100,7 @@ public sealed class TrueBodySolver
Crouch = Mathf.Clamp((standingPivot - pivotAboveFloor) / (CrouchRange * p.EyeHeight), 0f, 1f); Crouch = Mathf.Clamp((standingPivot - pivotAboveFloor) / (CrouchRange * p.EyeHeight), 0f, 1f);
float headPitchDown = Mathf.Max(0f, -Mathf.Asin(Mathf.Clamp(headFwd.Y, -1f, 1f))); float headPitchDown = Mathf.Max(0f, -Mathf.Asin(Mathf.Clamp(headFwd.Y, -1f, 1f)));
float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest; float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest;
Lean = lean;
ChestBasis = yawBasis * new Basis(Vector3.Right, -lean); ChestBasis = yawBasis * new Basis(Vector3.Right, -lean);
// Neck: from the head pivot down along the average of chest-up and head-up, sized so a // Neck: from the head pivot down along the average of chest-up and head-up, sized so a
@@ -96,6 +116,7 @@ public sealed class TrueBodySolver
// Pelvis: hangs below the upper chest along the trunk; follows through a critically damped // Pelvis: hangs below the upper chest along the trunk; follows through a critically damped
// spring so it lags quick head motion (hip sway) instead of being rigidly attached. // spring so it lags quick head motion (hip sway) instead of being rigidly attached.
Vector3 pelvisTarget = UpperChest - ChestBasis.Y * (TrunkRatio * p.Stature); Vector3 pelvisTarget = UpperChest - ChestBasis.Y * (TrunkRatio * p.Stature);
PelvisTarget = pelvisTarget;
if (!Initialized) if (!Initialized)
Pelvis = pelvisTarget; Pelvis = pelvisTarget;
float w = Mathf.Tau * PelvisFrequency; float w = Mathf.Tau * PelvisFrequency;
@@ -138,6 +159,7 @@ public sealed class TrueBodySolver
_arms[side].Solve(Shoulder[side], WristTarget[side], ChestBasis, outward, handBasis, p.UpperArm, p.Forearm, dt); _arms[side].Solve(Shoulder[side], WristTarget[side], ChestBasis, outward, handBasis, p.UpperArm, p.Forearm, dt);
Elbow[side] = _arms[side].Elbow; Elbow[side] = _arms[side].Elbow;
Pole[side] = _arms[side].Pole;
Wrist[side] = _arms[side].Wrist; Wrist[side] = _arms[side].Wrist;
} }
} }
@@ -157,6 +179,8 @@ public partial class TrueBody : Node3D
private readonly System.Collections.Generic.List<MeshInstance3D> _joints = new(); private readonly System.Collections.Generic.List<MeshInstance3D> _joints = new();
public TrueBodySolver Solver { get; } = new(); public TrueBodySolver Solver { get; } = new();
// Set by PlayerRig, which solves at the right point of its own tick (after moving the origin).
public bool ExternallySolved;
public bool HasPose => Solver.Initialized; public bool HasPose => Solver.Initialized;
public override void _Ready() public override void _Ready()
@@ -178,7 +202,7 @@ public partial class TrueBody : Node3D
public override void _PhysicsProcess(double delta) public override void _PhysicsProcess(double delta)
{ {
BodyProfile profile = BodyProfile.Current; BodyProfile profile = BodyProfile.Current;
if (profile == null) if (profile == null || ExternallySolved)
return; return;
Solver.Solve(profile, _camera.GlobalTransform, _left.GlobalTransform, _right.GlobalTransform, _origin.GlobalPosition.Y, (float)delta); Solver.Solve(profile, _camera.GlobalTransform, _left.GlobalTransform, _right.GlobalTransform, _origin.GlobalPosition.Y, (float)delta);
} }
scripts/WorldPanel.cs
+10
View File
@@ -79,6 +79,16 @@ public partial class WorldPanel : Node3D
SetPanelActive(false); SetPanelActive(false);
} }
// This controller currently points at (or touches) the menu — its stick scrolls the menu,
// so locomotion must ignore it.
public static bool IsPointing(XRController3D controller)
{
if (_instance == null || !_instance.Visible)
return false;
int hand = controller == _instance._left ? 0 : controller == _instance._right ? 1 : -1;
return hand >= 0 && _instance._screen.GetMode(hand) != MenuScreen.PointerMode.None;
}
public static bool IsBlocking(XRController3D controller) => public static bool IsBlocking(XRController3D controller) =>
_instance != null && _instance.Visible && _instance != null && _instance.Visible &&
_instance.GlobalPosition.DistanceTo(controller.GlobalPosition) < DirectGrabRadius; _instance.GlobalPosition.DistanceTo(controller.GlobalPosition) < DirectGrabRadius;