diff --git a/AGENTS.md b/AGENTS.md index 9b2414d..41b199c 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -306,6 +306,12 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x smooth turn, **physics 144 Hz with physics interpolation** (`Main.PhysicsRate`; XROrigin3D, TrueBody, PlayerRig are interpolation-OFF because they move per frame — anything new moved in `_Process` must be excluded too). Rule learned: play-space corrections must never feed ground control's feed-forward (runaway feedback). + **Step 2.2 (2.1 playtest was unstable — see research doc "Step 2.2"): RagdollSpike now runs REAL input + (90 Hz sample-and-hold tracking + jitter under 144 Hz physics); a body test on ideal input proves nothing.** + Tick pipeline = `RagdollDriver` (shared by PlayerRig and the spike; `PoseFilter` extrapolates tracking). + Rules: low-pass every feed-forward; muscles stay local to the TARGET pose (never the physical parent — rotation + feedback); no free-path view chasing while grounded; bodies whose shapes can all be disabled need explicit + inertia; Box3D shape queries return a null `collider` — use `collider_id`. 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.** `PhysicsArms.cs`: per side an upper-arm + forearm RigidBody3D (2.0 / 1.2 kg, layer 5) that diff --git a/HANDOFF.md b/HANDOFF.md index 3e0301b..f9c8012 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -103,6 +103,10 @@ TERMINOLOGY (owner-corrected — use precisely): static or arms lifting) = the only state that drags the view vertically, ground feed-forward that ignores play-space corrections (a runaway at 7 m/s otherwise), `ProceduralLegs` visuals, static-world grabs (climbing), menu strength slider / spectator / snap-smooth toggles, 144 Hz + physics interpolation (XR origin, TrueBody, PlayerRig excluded). +- **RagdollDriver.cs (step 2.2)** — `PoseFilter` (timestamps each new tracking sample, extrapolates between them in + play-space-local space; 90 Hz tracking under 144 Hz physics otherwise feeds jerks into every feed-forward) and + `RagdollDriver` (filter → OriginCorrection → True Body → build → Step), used by BOTH PlayerRig and RagdollSpike. + The 2.1 instabilities, their root causes and fixes: `research/ragdoll-player-step2.md` "Step 2.2". - **Playground.cs** — the body sandbox around the table (east climbing/mantling, west physics toys, south crawl/duck/door); dynamic pieces are "prop"s reset by the menu's Reset props. `tools/playground_shots.tscn` renders overview PNGs (`godot-mono --path . tools/playground_shots.tscn -- `). @@ -388,7 +392,7 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29). ## 5. Known issues / open questions - **Ragdoll player (step 2.1) — owner playtested step 2 ("feels amazing for the most part"); 2.1 fixes are - unvalidated in-headset.** Remaining gaps: shoulder has no cone limit (fork addition); strength values are + unvalidated in-headset; 2.1 was unstable in the headset, 2.2 fixes it — see research "Step 2.2".** Remaining gaps: shoulder has no cone limit (fork addition); strength values are human-scale guesses (menu slider); stick-crawling is a comfort concession (Nimso pulls with hands only); legs are visual only. Legacy spikes (arm/grab/body/collapse) pin 90 Hz because the chase bodies were tuned there. diff --git a/research/ragdoll-player-step2.md b/research/ragdoll-player-step2.md index 8ab10d0..790a9b3 100644 --- a/research/ragdoll-player-step2.md +++ b/research/ragdoll-player-step2.md @@ -92,3 +92,42 @@ Changes and the measured reasons: Ground steeper than `MaxSlopeDegrees` 35° gives no support, so the 40° ramp can't be walked up; the 18° one can. Still open: the shoulder cone limit (needs a fork addition). `tools/playground_shots.tscn` renders overview PNGs for layout checks. + +## Step 2.2 — the 2.1 playtest was unstable; root causes (2026-10-02) + +The owner reported everything "all over the place": +- Mantling swung the chest out of control and the hands "wobbled more and more". +- Crouching made the body slide around and spin. +- Nothing static was grabbable. +- The steep ramp slipped the body out from under them. + +RagdollSpike passed all of it, because its input was IDEAL: smooth poses every 144 Hz tick, no jitter. Fixes: + +1. **The spike now runs on REAL input by default**: tracking sample-and-hold at 90 Hz under 144 Hz physics, plus + 0.5 mm / 0.1° jitter per sample (`-- ideal` / `nonoise` / `nohold` isolate the factors). Lesson: a body-physics + test with perfect input proves nothing about the headset. +2. **`PoseFilter` + `RagdollDriver`** (`scripts/RagdollDriver.cs`). Tracked poses are timestamped per new sample + and extrapolated between samples in play-space-local coordinates. One driver runs the tick pipeline for + both `PlayerRig` and the spike, so the test runs the game's code path. +3. **Low-pass every feed-forward**: muscles/elbow τ 0.05 s, ground control τ 0.12 s. Ground control's velocity + term multiplies by the whole body mass, so 1.5 mm of jitter became hundreds of newtons of buzz (pelvis + 0.27 → 0.04 m/s at rest). +4. **Arm muscles stay LOCAL to the chest's target pose.** 2.1 aimed them in the physical chest frame, which closes a + rotation feedback loop: the chest turns, the world-fixed arm targets push back on the chest, and the chest turns + further. Jitter excited it into a slow twist (eye drifting 15 cm): the mantle "chest slipping". Only a + horizontal translation of the reach is kept (body held back from the camera). +5. **Structural hand strength only when the hand pushes on or holds something.** A hand reaching past its arm's + length into free air pulled the whole body down by up to 400 N (crouch: eye 11 cm off). +6. **No free-path view chasing while grounded.** Any eye/camera mismatch (crouch poses can't match exactly) moved + the view, the pose target followed, the body followed, and so on: an 8 m slide while holding a crouch. The view + is now only pushed back when geometry blocks the head. Instead, the stick stops pushing in the direction the + body is held back (`PlayerRagdoll.HeldBack`, `PlayerRig.HoldBack`). +7. **Explicit inertia on the `Legs` body.** Disabling its only collision shape (tuck) left it with no rotational + inertia; its 400 N·m drive then tumbled the pelvis forward, which flipped the "yaw". This was the crouch spin. + Rule: a body whose shapes can all be disabled needs explicit `Inertia`. +8. **World grabs: the Box3D wrapper returns a null `collider` from `IntersectShape`**, only `collider_id`. Resolve it + with `GodotObject.InstanceFromId`. The grab sphere is now 7 cm, checked at the palm and then the hand centre. + The spike asserts the game's own `PropGrabber.WorldUnderPalm` finds the bar. + +New spike phases: hold a crouch (eyes 1.0 m) and a deep crouch (0.7 m) without drift (< 5 cm) or spin (< 5°), +eye on camera (< 5 cm). 29 phases total, all on real input. diff --git a/scripts/PlayerRagdoll.cs b/scripts/PlayerRagdoll.cs index 4d019c9..8b3eb52 100644 --- a/scripts/PlayerRagdoll.cs +++ b/scripts/PlayerRagdoll.cs @@ -29,6 +29,10 @@ public partial class PlayerRagdoll : Node3D public const float ShoulderKp = 20f, ShoulderTorque = 45f; // ~human shoulder flexion strength public const float ElbowKp = 20f, ElbowTorque = 45f; public const float MuscleMaxSpin = 15f; + // Feed-forwards are derivatives of tracking: low-pass them or tracking jitter (±1–2 mm per + // sample) becomes hundreds of newtons of buzz (RagdollSpike REAL input mode). + public const float MuscleFeedForwardTau = 0.05f; // s + public const float GroundFeedForwardTau = 0.12f; // s 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. @@ -67,7 +71,7 @@ public partial class PlayerRagdoll : Node3D 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 FreeSlack = 0.2f; // camera leading the body's eye by more than this = body held back: stick stops pushing that way public const float AirSlack = 0.05f; // climbing/hanging (braced): the view rides the body tightly public const float FallSlack = 0.25f; // free airborne: brief ground loss (sliding off a steep slope) mustn't sink the view public const float GroundFollowRate = 8f; @@ -96,6 +100,7 @@ public partial class PlayerRagdoll : Node3D public float Kp; public Quaternion PrevTarget; public bool HasPrev; + public Vector3 Ff; } private sealed class Drive @@ -105,6 +110,7 @@ public partial class PlayerRagdoll : Node3D public Vector3 AnchorA, AnchorB; // anchor point, parent-local / child-local public Quaternion PrevTarget; public bool HasPrev; + public Vector3 Ff; } private sealed class Arm @@ -115,6 +121,7 @@ public partial class PlayerRagdoll : Node3D public Drive Shoulder, Guide; public float UpperLen, ForeLen; public float PrevFlexTarget = -1f; + public float FlexFf; public float GuideCap = HandGuideForce; public PhysicsBody3D Held; } @@ -126,6 +133,9 @@ public partial class PlayerRagdoll : Node3D public bool Grounded { get; private set; } public float GroundY => _footY; public bool Braced { get; private set; } // a hand touches or holds static world + // Horizontal direction in which the body is held back from the camera (zero if not): the stick + // must not keep moving the play space that way. + public Vector3 HeldBack { get; private set; } // Really climbing: a hand holds static world, or the arms are lifting the body (mantle). public bool Climbing => Built && (_arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D || _lifted); public RigidBody3D Legs { get; private set; } @@ -144,6 +154,9 @@ public partial class PlayerRagdoll : Node3D private readonly Vector3[] _girdle = new Vector3[2]; // True Body shoulder offset, chest frame private float _footY; private float _supportGoalY; + private Vector3 _goalVelocity; + + private static float LowPass(float dt, float tau) => 1f - Mathf.Exp(-dt / tau); private bool _lifted; private float _legsHeight; private Drive _legsDrive; @@ -198,6 +211,10 @@ public partial class PlayerRagdoll : Node3D _legsHeight = Mathf.Max(0.2f, standingHip - MaxStepHeight); Legs = MakeBody("Legs", new BoxShape3D { Size = new Vector3(0.28f * s, _legsHeight, 0.2f * s) }, LegsMass, BodyLayer, WorldMask, pants, c * new Transform3D(Basis.Identity, hip - new Vector3(0f, _legsHeight * 0.5f, 0f)), visible: false); + // Explicit inertia: the collider is switched off when tucked, and a body without an + // enabled shape has no inertia — its 400 N·m drive then tumbled the whole pelvis + // (owner 2.1 "body spinning" when crouched). + Legs.Inertia = new Vector3(_legsHeight * _legsHeight + 0.04f, 0.28f * 0.28f + 0.04f, _legsHeight * _legsHeight + 0.08f) * (LegsMass / 12f); _footY = pose.Eye.Y - p.EyeHeight; Chest = MakeBody("Chest", new BoxShape3D { Size = chestSize }, 0.216f * m, BodyLayer, WorldMask, shirt, c * new Transform3D(Basis.Identity, lumbar + _chestCenterLocal)); @@ -428,18 +445,23 @@ public partial class PlayerRagdoll : Node3D // real hand. Vertically a sagging body keeps its arm pose (the aim point moves with // the shoulder), so straight real arms stay straight virtual arms — pillars that hold // you up when crawling or press you up a ledge (the hand guide adds the rest). + // Only a horizontal TRANSLATION of the reach: solved in the chest TARGET frame (the + // muscles stay local). Aiming in the physical chest frame made the shoulder muscles + // feed chest rotation back into the chest — a slow twist that tracking jitter + // excited into "chest slipping out of control" (owner playtest 2.1). Transform3D chestPhys = Chest.GlobalTransform.Orthonormalized(); Vector3 shPhys = chestPhys * (a.Shoulder.AnchorA + _girdle[side]); + Vector3 held = shPhys - sh; + held.Y = 0f; Transform3D ctrl = _targets[a.Hand]; - Vector3 aim = ctrl * new Vector3(0f, 0f, WristOffset) + Vector3.Up * (shPhys.Y - sh.Y); ArmSolver solver = _armSolvers[side]; - solver.Solve(shPhys, aim, chestPhys.Basis, chestPhys.Basis.X * (side == 0 ? -1f : 1f), + solver.Solve(sh, ctrl * new Vector3(0f, 0f, WristOffset) - held, chest, chest.X * (side == 0 ? -1f : 1f), ctrl.Basis, a.UpperLen, a.ForeLen, dt); - Vector3 d = solver.Wrist - shPhys; - Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chestPhys.Basis.X; - h = h.LengthSquared() > 1e-8f ? h.Normalized() : chestPhys.Basis.X; - Vector3 u = (solver.Elbow - shPhys).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized(); - _targets[a.Upper] = new Transform3D(BoneBasis(u, h), shPhys + u * (a.UpperLen * 0.5f)); + Vector3 d = solver.Wrist - sh; + Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chest.X; + h = h.LengthSquared() > 1e-8f ? h.Normalized() : chest.X; + Vector3 u = (solver.Elbow - sh).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized(); + _targets[a.Upper] = new Transform3D(BoneBasis(u, h), sh + u * (a.UpperLen * 0.5f)); _targets[a.Fore] = new Transform3D(BoneBasis(f, h), solver.Elbow + f * (a.ForeLen * 0.5f)); } } @@ -502,10 +524,11 @@ public partial class PlayerRagdoll : Node3D 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; + Vector3 raw = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; + b.Ff = b.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); b.PrevTarget = target; b.HasPrev = true; - Vector3 w = (ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin); + Vector3 w = (b.Ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin); SetAngularVelocity(b.Joint, w); } @@ -537,9 +560,10 @@ public partial class PlayerRagdoll : Node3D 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; + float raw = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f; + a.FlexFf = Mathf.Lerp(a.FlexFf, raw, LowPass(dt, MuscleFeedForwardTau)); a.PrevFlexTarget = target; - float w = Mathf.Clamp(ff + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin); + float w = Mathf.Clamp(a.FlexFf + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin); a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w); } @@ -622,13 +646,15 @@ public partial class PlayerRagdoll : Node3D SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp)); Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB); - // Arm targets are solved from the physical chest, so the physical chest is the parent frame. - Quaternion target = (chest.Basis.GetRotationQuaternion() * d.FrameA).Inverse() + // Local muscle: relative to the chest's TARGET pose (never to the physical chest — that + // closes a rotation feedback loop through the arms). + 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; + Vector3 raw = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; + d.Ff = d.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); d.PrevTarget = target; d.HasPrev = true; - SetAngularVelocity(d.Joint, (ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin)); + SetAngularVelocity(d.Joint, (d.Ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin)); } // Force-capped pull of the hand onto the controller, reacting into the chest (arm muscles). @@ -645,6 +671,10 @@ public partial class PlayerRagdoll : Node3D // Axial share of the error, with a 3 cm floor so a nearly-reached target's noisy error // direction can't flip a held load into "structural". float align = axis.LengthSquared() > 1e-8f ? Mathf.Abs(err.Dot(axis.Normalized())) / (err.Length() + StructuralDeadband) : 0f; + // Structure only helps against something: a hand in free air reaching past its arm's + // length must not haul the whole body after it. + if (a.Held == null && a.Hand.GetContactCount() == 0) + align = 0f; float cap = Mathf.Lerp(HandGuideForce, HandStructuralForce, align) * Mathf.Max(0.05f, Strength); if (Mathf.Abs(cap - a.GuideCap) > 1f) { @@ -701,11 +731,13 @@ public partial class PlayerRagdoll : Node3D // ground, the pose target and the ground offset move by the same amount in opposite // directions — differentiating the pose alone would launch the body. Vector3 goalNow = target.Origin + Vector3.Up * (_footY - floorY); - Vector3 targetVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero; + Vector3 rawVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero; _pelvisTargetPrev = goalNow; _havePelvisPrev = true; - if (targetVelocity.Length() > 10f) - targetVelocity = Vector3.Zero; + if (rawVelocity.Length() > 10f) + rawVelocity = Vector3.Zero; + _goalVelocity = _goalVelocity.Lerp(rawVelocity, LowPass(dt, GroundFeedForwardTau)); + Vector3 targetVelocity = _goalVelocity; float weight = BodyMass * 9.8f; if (Grounded) @@ -778,13 +810,20 @@ public partial class PlayerRagdoll : Node3D if (!Climbing) correction.Y = 0f; } - else + else if (!Grounded) { - float slack = Grounded ? FreeSlack : Climbing ? AirSlack : FallSlack; - if (lead.Length() > slack) - correction = -lead * (1f - slack / lead.Length()); + // Free path: the view never chases the body while standing — any pose mismatch + // (a crouch the body can't match exactly) would otherwise walk the play space away + // forever (body → view → pose target → body; owner 2.1 "sliding around"). + // Airborne it rides the body: vertically in a fall, fully while climbing. + Vector3 follow = Climbing ? lead : new Vector3(0f, lead.Y, 0f); + float slack = Climbing ? AirSlack : FallSlack; + if (follow.Length() > slack) + correction = -follow * (1f - slack / follow.Length()); } } + Vector3 flatLead = new(lead.X, 0f, lead.Z); + HeldBack = flatLead.Length() > FreeSlack ? flatLead.Normalized() : Vector3.Zero; if (Grounded) { correction.Y = 0f; diff --git a/scripts/PlayerRig.cs b/scripts/PlayerRig.cs index a8800fa..c4241c4 100644 --- a/scripts/PlayerRig.cs +++ b/scripts/PlayerRig.cs @@ -33,9 +33,9 @@ public partial class PlayerRig : Node 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; + private RagdollDriver _driver; public PlayerRagdoll Ragdoll => _ragdoll; public static bool Spectator { get; private set; } @@ -84,6 +84,7 @@ public partial class PlayerRig : Node // frame); the body is simulated, so it interpolates. _ragdoll = new PlayerRagdoll { Name = "Ragdoll", PhysicsInterpolationMode = PhysicsInterpolationModeEnum.On }; AddChild(_ragdoll); + _driver = new RagdollDriver { Solver = Solver, Ragdoll = _ragdoll }; BodyProfile.Changed += _ => _rebuild = true; for (int i = 0; i < 2; i++) { @@ -121,30 +122,16 @@ public partial class PlayerRig : Node if (_haveLastOrigin && _ragdoll.Built && IsJump(_lastOrigin, origin)) Carry(origin * _lastOrigin.AffineInverse()); - if (_ragdoll.Built && !_rebuild) - { - // 2. Stick locomotion, 3. reconciliation. + // 2. Stick locomotion. + if (_ragdoll.Built && !_driver.RebuildRequested) 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); + // 3. Filtered tracking → play-space correction → True Body → muscles (shared with RagdollSpike). + _driver.RebuildRequested |= _rebuild; + _rebuild = false; + Transform3D o = _origin.GlobalTransform; + _driver.Tick(profile, ref o, _camera.Transform, _left.Transform, _right.Transform, _moveVelocity, dt); + _origin.GlobalTransform = o; _lastOrigin = _origin.GlobalTransform; _haveLastOrigin = true; } @@ -162,6 +149,14 @@ public partial class PlayerRig : Node } } + // The body is held back (hips against a table, a wall the head clears): stop the stick from + // carrying the view further that way. + public static Vector3 HoldBack(Vector3 velocity, Vector3 heldBack) + { + float into = velocity.Dot(heldBack); + return into > 0f ? velocity - heldBack * into : velocity; + } + private static bool IsJump(Transform3D a, Transform3D b) => a.Origin.DistanceTo(b.Origin) > JumpDistance || BodyMath.RotationError(b.Basis.GetRotationQuaternion(), a.Basis.GetRotationQuaternion()).Length() > JumpAngle; @@ -170,8 +165,6 @@ public partial class PlayerRig : Node { _ragdoll.TeleportBy(delta); Solver.Shift(delta); - _ctrlPrev[0] = delta * _ctrlPrev[0]; - _ctrlPrev[1] = delta * _ctrlPrev[1]; } private void Locomote(float dt) @@ -195,6 +188,7 @@ public partial class PlayerRig : Node if (!_ragdoll.Grounded) desired = _moveVelocity; // no air control _moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt); + _moveVelocity = HoldBack(_moveVelocity, _ragdoll.HeldBack); if (_moveVelocity != Vector3.Zero) _origin.GlobalPosition += _moveVelocity * dt; diff --git a/scripts/PropGrabber.cs b/scripts/PropGrabber.cs index 84ebe45..e7b9cb1 100644 --- a/scripts/PropGrabber.cs +++ b/scripts/PropGrabber.cs @@ -11,7 +11,7 @@ public partial class PropGrabber : Node private const float BreakTime = 0.25f; private const float BreakDistanceBoth = 0.4f; private const float GripRipDistance = 0.8f; - private const float WorldGrabRadius = 0.05f; // palm sphere for grabbing static world (ledges, bars, holds) + private const float WorldGrabRadius = 0.07f; // palm sphere for grabbing static world (ledges, bars, holds) public const string NoGrabGroup = "no_grab"; // static bodies that can't be grabbed (the floor) private static PropGrabber _instance; @@ -197,7 +197,7 @@ public partial class PropGrabber : Node _breakTimer[hand] = 0f; } - private static Vector3 PalmPoint(RigidBody3D handBody, int hand) => + public static Vector3 PalmPoint(RigidBody3D handBody, int hand) => handBody.GlobalPosition + handBody.GlobalTransform.Basis * new Vector3(PalmOffset(hand), 0f, 0f); private static float PalmOffset(int hand) => hand == 0 ? PalmGrabOffset : -PalmGrabOffset; @@ -214,11 +214,12 @@ public partial class PropGrabber : Node nearest = NearestProp(handBody.GlobalPosition, CenterGrabRadius); if (nearest != null) return nearest; - return WorldUnderPalm(palm, handBody.GetWorld3D()); + return WorldUnderPalm(palm, handBody.GetWorld3D()) ?? WorldUnderPalm(handBody.GlobalPosition, handBody.GetWorld3D()); } - // Climbing: any static body touching the palm (except the "no_grab" group) can be held. - private static StaticBody3D WorldUnderPalm(Vector3 palm, World3D world) + // Climbing: any static body near the palm (except the "no_grab" group) can be held. + // NOTE (Box3D wrapper): shape-query results carry a null "collider" — resolve "collider_id". + public static StaticBody3D WorldUnderPalm(Vector3 palm, World3D world) { PhysicsShapeQueryParameters3D query = new() { @@ -228,7 +229,7 @@ public partial class PropGrabber : Node }; foreach (Godot.Collections.Dictionary hit in world.DirectSpaceState.IntersectShape(query, 8)) { - if (hit["collider"].AsGodotObject() is StaticBody3D body && !body.IsInGroup(NoGrabGroup)) + if (GodotObject.InstanceFromId((ulong)hit["collider_id"]) is StaticBody3D body && !body.IsInGroup(NoGrabGroup)) return body; } return null; diff --git a/scripts/RagdollDriver.cs b/scripts/RagdollDriver.cs new file mode 100644 index 0000000..8763a8b --- /dev/null +++ b/scripts/RagdollDriver.cs @@ -0,0 +1,93 @@ +using Godot; + +// Smooths one tracked pose for the physics tick. Tracking updates at the display rate (90 Hz) but +// physics runs at 144 Hz: sampled raw, every second or third tick sees the pose frozen and the next +// one jumped, and every velocity feed-forward downstream (muscles, hand guides, ground control) +// turns that into jerks — the step-2.1 "wobbling more and more / sliding around" in the headset. +// Each new sample is timestamped; between samples the pose is extrapolated with the (smoothed) +// sample velocity. Works in the PLAY-SPACE-LOCAL frame, so moving the origin never looks like motion. +public sealed class PoseFilter +{ + public const float MaxExtrapolation = 1.5f / 90f; // s + public const float VelocitySmoothing = 0.3f; // EMA weight of each new sample + public const float JumpDistance = 0.5f; // m between samples = recenter, not motion + + private Transform3D _last; + private double _tLast; + private bool _has; + + public Vector3 Velocity { get; private set; } // play-space-local m/s + public Vector3 AngularVelocity { get; private set; } // play-space-local rad/s + + public void Reset() => _has = false; + + public Transform3D Sample(Transform3D raw, double now) + { + if (!_has) + { + _has = true; + _last = raw; + _tLast = now; + Velocity = AngularVelocity = Vector3.Zero; + return raw; + } + if (raw != _last) + { + float interval = (float)(now - _tLast); + if (interval > 1e-4f) + { + if (raw.Origin.DistanceTo(_last.Origin) > JumpDistance) + { + Velocity = AngularVelocity = Vector3.Zero; + } + else + { + Vector3 v = (raw.Origin - _last.Origin) / interval; + Vector3 w = BodyMath.RotationError(raw.Basis.GetRotationQuaternion(), _last.Basis.GetRotationQuaternion()) / interval; + Velocity = Velocity.Lerp(v, VelocitySmoothing); + AngularVelocity = AngularVelocity.Lerp(w, VelocitySmoothing); + } + } + _last = raw; + _tLast = now; + } + float age = Mathf.Min((float)(now - _tLast), MaxExtrapolation); + Basis spin = AngularVelocity.LengthSquared() > 1e-10f + ? new Basis(AngularVelocity.Normalized(), AngularVelocity.Length() * age) : Basis.Identity; + return new Transform3D(spin * _last.Basis.Orthonormalized(), _last.Origin + Velocity * age); + } +} + +// The per-physics-tick body pipeline, shared by PlayerRig (headset) and RagdollSpike so the +// regression runs exactly what the game runs: filter the tracked poses → play-space correction +// (push-back / ground follow / falls) → True Body solve → (re)build → ragdoll muscles. +// Locomotion (moving the origin by the stick, turning, teleports) happens before Tick. +public sealed class RagdollDriver +{ + public readonly PoseFilter Head = new(), Left = new(), Right = new(); + public TrueBodySolver Solver; + public PlayerRagdoll Ragdoll; + public bool RebuildRequested = true; + private double _now; + + // origin = play space (modified by the correction); raw* = origin-local tracked poses as last + // reported (repeat between tracking updates); moveVelocity = intended origin velocity (stick), + // the only origin motion that is real hand motion for the guides' feed-forward. + public void Tick(BodyProfile profile, ref Transform3D origin, Transform3D rawHead, Transform3D rawLeft, Transform3D rawRight, + Vector3 moveVelocity, float dt) + { + _now += dt; + Transform3D head = Head.Sample(rawHead, _now), left = Left.Sample(rawLeft, _now), right = Right.Sample(rawRight, _now); + if (Ragdoll.Built && !RebuildRequested) + origin.Origin += Ragdoll.OriginCorrection((origin * head).Origin, origin.Origin.Y, dt); + Transform3D h = origin * head, l = origin * left, r = origin * right; + Solver.Solve(profile, h, l, r, origin.Origin.Y, dt); + if (RebuildRequested || !Ragdoll.Built) + { + Ragdoll.Build(profile, Solver, l, r); + RebuildRequested = false; + } + Vector3 lv = origin.Basis * Left.Velocity + moveVelocity, rv = origin.Basis * Right.Velocity + moveVelocity; + Ragdoll.Step(Solver, l, r, lv, rv, origin.Origin.Y, dt); + } +} diff --git a/scripts/RagdollSpike.cs b/scripts/RagdollSpike.cs index b0464f3..5d02ed1 100644 --- a/scripts/RagdollSpike.cs +++ b/scripts/RagdollSpike.cs @@ -23,8 +23,6 @@ public partial class RagdollSpike : Node3D 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; @@ -32,13 +30,49 @@ public partial class RagdollSpike : Node3D private int _snapsAtStart; private float _measureFrom; - private Transform3D Camera => _origin * _headLocal; - private Transform3D Left => _origin * _leftLocal; - private Transform3D Right => _origin * _rightLocal; + // What the ragdoll sees. REAL-INPUT mode (default; `-- ideal` disables it) reproduces the headset: + // tracking poses update at the 90 Hz display rate (sample-and-hold between 144 Hz physics ticks) + // with a little jitter — the smooth per-tick input of the ideal mode hid the 2.1 instabilities. + private Transform3D Camera => _origin * Tracked(0, _headLocal); + private Transform3D Left => _origin * Tracked(1, _leftLocal); + private Transform3D Right => _origin * Tracked(2, _rightLocal); + + private bool _realInput = true, _noiseOn = true, _holdOn = true; + private const float TrackingHz = 90f; + private readonly Transform3D[] _held = new Transform3D[3]; + private readonly long[] _heldFrame = { -1, -1, -1 }; + private readonly RandomNumberGenerator _noise = new() { Seed = 3 }; + private float _clock; + + private Transform3D Tracked(int i, Transform3D truth) + { + if (!_realInput) + return truth; + long frame = _holdOn ? (long)(_clock * TrackingHz) : (long)(_clock * 100000f); + if (frame != _heldFrame[i]) + { + _heldFrame[i] = frame; + float p = _noiseOn ? 0.0005f : 0f, r = _noiseOn ? Mathf.DegToRad(0.1f) : 0f; // pessimistic for inside-out tracking + Vector3 dp = new(_noise.Randfn(0f, p), _noise.Randfn(0f, p), _noise.Randfn(0f, p)); + Basis dr = Basis.FromEuler(new Vector3(_noise.Randfn(0f, r), _noise.Randfn(0f, r), _noise.Randfn(0f, r))); + _held[i] = new Transform3D(dr * truth.Basis, truth.Origin + dp); + } + return _held[i]; + } public override void _Ready() { _profile = BodyProfile.Estimate(EyeHeight); + foreach (string arg in OS.GetCmdlineUserArgs()) + { + if (arg == "ideal") + _realInput = false; + if (arg == "nonoise") + _noiseOn = false; + if (arg == "nohold") + _holdOn = false; + } + GD.Print($"RAGDOLL: input mode {(_realInput ? "REAL (90 Hz tracking, jitter)" : "ideal")} at {Engine.PhysicsTicksPerSecond} Hz physics"); AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29)); _ragdoll = new PlayerRagdoll { Name = "Ragdoll" }; AddChild(_ragdoll); @@ -65,6 +99,8 @@ public partial class RagdollSpike : Node3D _phases.Add(("two-handed pull-up on a grabbed bar", () => SetupBarHang(), t => TickPullUp(both: true), 3.5f, () => FinishPullUp(true))); _phases.Add(("one-handed pull-up is weaker", () => SetupBarHang(), t => TickPullUp(both: false), 3.5f, () => FinishPullUp(false))); _phases.Add(("crawls through a 0.6 m tunnel", SetupTunnel, TickCrawl, 7f, FinishCrawl)); + _phases.Add(("holds a crouch (eyes 1.0 m) without sliding or spinning", Rest, t => TickCrouchHold(1.0f), 5f, FinishCrouchHold)); + _phases.Add(("holds a deep crouch (eyes 0.7 m) without sliding or spinning", Rest, t => TickCrouchHold(0.7f), 5f, FinishCrouchHold)); _phases.Add(("walks off a 1 m ledge and lands", SetupLedgeTop, _ => _stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero, 4f, FinishDrop)); _phases.Add(("walks up an 18° ramp", () => SetupRamp(18f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(true))); _phases.Add(("can't walk up a 40° ramp", () => SetupRamp(40f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(false))); @@ -79,6 +115,7 @@ public partial class RagdollSpike : Node3D return; float dt = (float)delta; _t += dt; + _clock += dt; _phases[_phase].Tick(dt); if (_phase > 0) Drive(dt); @@ -116,24 +153,34 @@ public partial class RagdollSpike : Node3D // 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); + _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll }; + Vector3 stick = _ragdoll.Built && !_needBuild ? PlayerRig.HoldBack(_stickVelocity, _ragdoll.HeldBack) : _stickVelocity; + _origin.Origin += stick * dt; + _driver.Tick(_profile, ref _origin, Tracked(0, _headLocal), Tracked(1, _leftLocal), Tracked(2, _rightLocal), _stickVelocity, dt); Measure(); } + private RagdollDriver _driver; + private bool _needBuild + { + get => _driver == null || _driver.RebuildRequested; + set + { + _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll }; + _driver.RebuildRequested = value; + if (value) + { + // New phase: resample tracking now (a held sample from the last phase would build + // the body where the previous phase left the head). + for (int i = 0; i < 3; i++) + _heldFrame[i] = -1; + _driver.Head.Reset(); + _driver.Left.Reset(); + _driver.Right.Reset(); + } + } + } + private void Measure() { foreach (RigidBody3D b in _ragdoll.Bodies) @@ -224,8 +271,10 @@ public partial class RagdollSpike : Node3D private bool FinishStand() { + float core = Mathf.Max(_ragdoll.Pelvis.LinearVelocity.Length(), Mathf.Max(_ragdoll.Chest.LinearVelocity.Length(), _ragdoll.Head.LinearVelocity.Length())); + GD.Print($"RAGDOLL: body core speed at rest {core:F3} m/s (limit 0.03)"); 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); + return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && core < 0.03f && _maxSpeed < 0.15f && _maxElbowErr < Mathf.DegToRad(8f); } private readonly List _reachLog = new(); @@ -435,8 +484,6 @@ public partial class RagdollSpike : Node3D Transform3D delta = _origin * before.AffineInverse(); _ragdoll.TeleportBy(delta); _solver.Shift(delta); - _prevL = delta * _prevL; - _prevR = delta * _prevR; _teleported = true; } if (!_turned && _t > 0.9f) @@ -446,8 +493,6 @@ public partial class RagdollSpike : Node3D _origin = rot * _origin; _ragdoll.TeleportBy(rot); _solver.Shift(rot); - _prevL = rot * _prevL; - _prevR = rot * _prevR; _turned = true; } } @@ -552,6 +597,18 @@ public partial class RagdollSpike : Node3D _leftLocal = both ? new Transform3D(grip, new Vector3(-0.22f, y, BarHangZ + 0.03f)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f)); if (!_barGrabbed && t > 0.6f && _ragdoll.Built) { + // The game's own world-grab detection must find the bar under the palm. + if (both) + { + for (int side = 0; side < 2; side++) + { + RigidBody3D hand = _ragdoll.Hand(side); + Vector3 palm = PropGrabber.PalmPoint(hand, side); + StaticBody3D found = PropGrabber.WorldUnderPalm(palm, hand.GetWorld3D()) ?? PropGrabber.WorldUnderPalm(hand.GlobalPosition, hand.GetWorld3D()); + _grabDetect &= found == _bar; + GD.Print($"RAGDOLL: world grab detection side {side}: palm {palm}, bar {_bar.GlobalPosition}, found {(found == null ? "nothing" : found.Name)}"); + } + } _barGrabbed = true; _barPelvisStart = _ragdoll.Pelvis.GlobalPosition.Y; for (int side = both ? 0 : 1; side < 2; side++) @@ -567,6 +624,7 @@ public partial class RagdollSpike : Node3D } private float _twoHandRise; + private bool _grabDetect = true; private bool FinishPullUp(bool both) { @@ -576,7 +634,8 @@ public partial class RagdollSpike : Node3D if (both) { _twoHandRise = _maxPelvisRise; - return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f; + GD.Print($"RAGDOLL: world grab detection {(_grabDetect ? "found the bar" : "FAILED")}"); + return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f && _grabDetect; } return _finite && NewSnaps == 0 && _maxPelvisRise < _twoHandRise * 0.6f; } @@ -630,6 +689,42 @@ public partial class RagdollSpike : Node3D && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > -3f; } + private Vector3 _crouchPelvis; + private float _crouchYaw, _crouchDrift, _crouchSpin; + + // Squat down over 1.5 s (looking a bit down, hands in front of the knees) and hold. + private void TickCrouchHold(float eye) + { + float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f)); + float y = Mathf.Lerp(EyeHeight, eye, k); + _headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, y, -0.1f * k)); + _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f)); + _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f)); + _measureFrom = float.MaxValue; + if (!_ragdoll.Built || _needBuild) + return; + Vector3 f = -_ragdoll.Pelvis.GlobalTransform.Basis.Z; + float yaw = Mathf.Atan2(-f.X, -f.Z); + if (_t < 2f) + { + _crouchPelvis = _ragdoll.Pelvis.GlobalPosition; + _crouchYaw = yaw; + _crouchDrift = _crouchSpin = 0f; + return; + } + Vector3 d = _ragdoll.Pelvis.GlobalPosition - _crouchPelvis; + d.Y = 0f; + _crouchDrift = Mathf.Max(_crouchDrift, d.Length()); + _crouchSpin = Mathf.Max(_crouchSpin, Mathf.Abs(Mathf.AngleDifference(_crouchYaw, yaw))); + } + + private bool FinishCrouchHold() + { + GD.Print($"RAGDOLL: crouch hold: pelvis drift {_crouchDrift:F3} m, yaw change {Mathf.RadToDeg(_crouchSpin):F1}°, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, origin {_origin.Origin}, snaps {NewSnaps}"); + return _finite && NewSnaps == 0 && _crouchDrift < 0.05f && _crouchSpin < Mathf.DegToRad(5f) && _origin.Origin.Length() < 0.05f + && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.05f; + } + // A slope rising away (−Z) from ObstacleFront up to 1 m, then a platform. private void SetupRamp(float degrees) {