FoxyzandClaude Opus 5.5
84df772f38
- one 3D view-follow rule (2 cm deadzone, hard 4 cm limit), no separate floor-follow; play-space-relative targets mean no feedback loop - head guide drives the character's eye to the HMD while on your feet - slow floor re-centering; Braced ignores the floor; anatomical head box Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
1041 lines
50 KiB
C#
1041 lines
50 KiB
C#
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;
|
||
|
||
// Measurements (reset per phase).
|
||
private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
|
||
private bool _finite;
|
||
private int _snapsAtStart;
|
||
private float _measureFrom;
|
||
|
||
// 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");
|
||
StaticBody3D floor = MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29);
|
||
floor.AddToGroup(PropGrabber.NoGrabGroup); // like main.tscn's floor
|
||
AddChild(floor);
|
||
_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));
|
||
_phases.Add(("a knee-high box blocks walking (no stepping onto it)", () => SetupObstacle(0.6f), _ => _stickVelocity = new Vector3(0f, 0f, -1.5f), 3f, FinishBlockedLow));
|
||
_phases.Add(("a table can't be walked over", SetupTableObstacle, _ => _stickVelocity = new Vector3(0f, 0f, -1.5f), 3f, FinishBlockedLow));
|
||
_phases.Add(("walks up a 0.25 m step", () => SetupObstacle(0.25f, 8f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishStep));
|
||
_phases.Add(("two-handed mantle onto a 0.9 m ledge (crouch, hands on the edge, stand and press)", () => SetupObstacle(0.9f), _ => TickHumanMantle(both: true), 7f, () => FinishMantle(true)));
|
||
_phases.Add(("one hand can't mantle", () => SetupObstacle(0.9f), _ => TickHumanMantle(both: false), 7f, () => FinishMantle(false)));
|
||
_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(("holds poses (stand, crouch, deep crouch, lean, reach, kneel) without view/body drift", Rest, TickPoseHold, 18f, FinishPoseHold));
|
||
_phases.Add(("hanging from a bar keeps the view on the body", () => SetupBarHang(), TickHang, 4f, FinishHang));
|
||
_phases.Add(("climbs a wall hand over hand (grab, pull, regrab higher)", SetupClimbWall, TickClimbWall, 8f, FinishClimbWall));
|
||
_phases.Add(("walks off a 1 m ledge and lands without sinking away from the view", SetupLedgeTop, TickDrop, 4f, FinishDrop));
|
||
_phases.Add(("standing on a seesaw tips it (body weight reaches movable ground)", SetupSeesaw, _ => { }, 3f, FinishSeesaw));
|
||
_phases.Add(("playground: swing board hangs, seesaw on its pivot, big plank grabbable", SetupPlayground, _ => { }, 2f, FinishPlayground));
|
||
_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)));
|
||
Next();
|
||
}
|
||
|
||
// ---------------------------------------------------------------- harness
|
||
|
||
public override void _PhysicsProcess(double delta)
|
||
{
|
||
if (_phase >= _phases.Count)
|
||
return;
|
||
float dt = (float)delta;
|
||
_t += dt;
|
||
_clock += 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)
|
||
{
|
||
_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)
|
||
_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()
|
||
{
|
||
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 && core < 0.03f && _maxSpeed < 0.15f && _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);
|
||
_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);
|
||
_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;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- obstacles, mantling, climbing, crawling
|
||
|
||
private const float ObstacleFront = -0.55f;
|
||
|
||
// A box from ObstacleFront 2 m deep (wide), `height` tall.
|
||
private void SetupObstacle(float height, float depth = 2f)
|
||
{
|
||
Rest();
|
||
_stage.AddChild(MakeStatic(new Vector3(3f, height, depth), new Vector3(0f, height * 0.5f, ObstacleFront - depth * 0.5f), 29));
|
||
}
|
||
|
||
private void SetupTableObstacle()
|
||
{
|
||
Rest();
|
||
_stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, ObstacleFront - 0.4f), 29));
|
||
foreach (float x in new[] { -0.65f, 0.65f })
|
||
foreach (float z in new[] { ObstacleFront - 0.05f, ObstacleFront - 0.75f })
|
||
_stage.AddChild(MakeStatic(new Vector3(0.06f, 0.7f, 0.06f), new Vector3(x, 0.35f, z), 29));
|
||
}
|
||
|
||
private bool FinishBlockedLow()
|
||
{
|
||
GD.Print($"RAGDOLL: obstacle front z {ObstacleFront}: pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _ragdoll.GroundY < 0.1f && _origin.Origin.Y < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > ObstacleFront - 0.3f;
|
||
}
|
||
|
||
private bool FinishStep()
|
||
{
|
||
GD.Print($"RAGDOLL: step: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && Mathf.Abs(_ragdoll.GroundY - 0.25f) < 0.03f && _origin.Origin.Y > 0.2f && _ragdoll.Pelvis.GlobalPosition.Z < ObstacleFront - 0.3f;
|
||
}
|
||
|
||
private float _maxPelvisRise, _lastRise;
|
||
|
||
// Put both hands on the ledge, press down (controllers sink as the real arms straighten), then
|
||
// walk forward onto it.
|
||
private bool FinishMantle(bool both)
|
||
{
|
||
GD.Print($"RAGDOLL: mantle ({(both ? "two hands" : "one hand")}): pelvis rose up to {_maxPelvisRise:F3} m, feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
|
||
bool onTop = _ragdoll.GroundY > 0.85f && _origin.Origin.Y > 0.8f;
|
||
_lastRise = _maxPelvisRise;
|
||
_maxPelvisRise = 0f;
|
||
return _finite && NewSnaps == 0 && (both ? onTop : !onTop); // standing tall also lifts the hips; one hand never gets you up
|
||
}
|
||
|
||
private StaticBody3D _bar;
|
||
private const float BarY = 1.95f, BarHangZ = -0.15f;
|
||
private float _barPelvisStart;
|
||
private bool _barGrabbed;
|
||
|
||
private void SetupBarHang()
|
||
{
|
||
Rest();
|
||
_bar = new StaticBody3D { Position = new Vector3(0f, BarY, BarHangZ), CollisionMask = 29 };
|
||
_bar.AddChild(new CollisionShape3D { Shape = new CylinderShape3D { Radius = 0.025f, Height = 2f }, RotationDegrees = new Vector3(0f, 0f, 90f) });
|
||
_stage.AddChild(_bar);
|
||
_barGrabbed = false;
|
||
_maxPelvisRise = 0f;
|
||
}
|
||
|
||
// Reach up to the bar, grip it (the PropGrabber weld), then pull the controllers down.
|
||
private void TickPullUp(bool both)
|
||
{
|
||
float t = _t;
|
||
float pull = Mathf.Clamp((t - 0.8f) / 1.5f, 0f, 1f);
|
||
float y = Mathf.Lerp(BarY - 0.03f, BarY - 0.75f, pull);
|
||
Basis grip = Aim(Vector3.Up + Vector3.Forward * 0.2f);
|
||
_rightLocal = new Transform3D(grip, new Vector3(0.22f, y, BarHangZ + 0.03f));
|
||
_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));
|
||
bool handsAtBar = _ragdoll.Built && !_needBuild
|
||
&& _ragdoll.Hand(1).GlobalPosition.DistanceTo(_bar.GlobalPosition + new Vector3(0.22f, 0f, 0f)) < 0.1f
|
||
&& (!both || _ragdoll.Hand(0).GlobalPosition.DistanceTo(_bar.GlobalPosition + new Vector3(-0.22f, 0f, 0f)) < 0.1f);
|
||
if (!_barGrabbed && t > 0.3f && handsAtBar)
|
||
{
|
||
// 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++)
|
||
{
|
||
RigidBody3D hand = _ragdoll.Hand(side);
|
||
ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _bar.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
|
||
_stage.AddChild(weld);
|
||
_ragdoll.SetHeld(side, _bar);
|
||
}
|
||
}
|
||
if (_barGrabbed)
|
||
_maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - _barPelvisStart);
|
||
}
|
||
|
||
private float _twoHandRise;
|
||
private bool _grabDetect = true;
|
||
|
||
private bool FinishPullUp(bool both)
|
||
{
|
||
GD.Print($"RAGDOLL: pull-up ({(both ? "two hands" : "one hand")}): pelvis rose {_maxPelvisRise:F3} m, braced {_ragdoll.Braced}, snaps {NewSnaps}");
|
||
_ragdoll.SetHeld(0, null);
|
||
_ragdoll.SetHeld(1, null);
|
||
if (both)
|
||
{
|
||
_twoHandRise = _maxPelvisRise;
|
||
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;
|
||
}
|
||
|
||
private const float TunnelStart = -1.0f, TunnelEnd = -3.5f;
|
||
private float _minCamY;
|
||
|
||
private void SetupTunnel()
|
||
{
|
||
Rest();
|
||
float len = TunnelStart - TunnelEnd;
|
||
float mid = (TunnelStart + TunnelEnd) * 0.5f;
|
||
_stage.AddChild(MakeStatic(new Vector3(1.4f, 0.1f, len), new Vector3(0f, 0.65f, mid), 29));
|
||
_stage.AddChild(MakeStatic(new Vector3(0.1f, 0.6f, len), new Vector3(-0.65f, 0.3f, mid), 29));
|
||
_stage.AddChild(MakeStatic(new Vector3(0.1f, 0.6f, len), new Vector3(0.65f, 0.3f, mid), 29));
|
||
_minCamY = float.MaxValue;
|
||
}
|
||
|
||
// Get down on hands and knees (eyes ~0.4 m, looking ahead), then stick-crawl forward.
|
||
private void TickCrawl(float dt)
|
||
{
|
||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 2f, 1f));
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.35f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 0.42f, k), Mathf.Lerp(0f, -0.25f, k)));
|
||
_leftLocal = new Transform3D(Aim(Vector3.Forward + Vector3.Down), new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.12f, k), Mathf.Lerp(-0.3f, -0.45f, k)));
|
||
_rightLocal = new Transform3D(Aim(Vector3.Forward + Vector3.Down), new Vector3(0.2f, Mathf.Lerp(1.0f, 0.12f, k), Mathf.Lerp(-0.3f, -0.45f, k)));
|
||
_stickVelocity = _t > 2.3f ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero;
|
||
_measureFrom = float.MaxValue;
|
||
if (_t > 2.3f)
|
||
_minCamY = Mathf.Min(_minCamY, Camera.Origin.Y);
|
||
}
|
||
|
||
private bool FinishCrawl()
|
||
{
|
||
GD.Print($"RAGDOLL: crawl: camera min z {_minCamZ:F3} (tunnel {TunnelStart}..{TunnelEnd}), eye y {_ragdoll.Eye.Y:F2}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F2}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _minCamZ < TunnelStart - 1.5f && Mathf.Abs(_origin.Origin.Y) < 0.1f;
|
||
}
|
||
|
||
|
||
// Start standing on a 1 m platform whose edge is 0.5 m ahead.
|
||
private void SetupLedgeTop()
|
||
{
|
||
Rest();
|
||
_origin = new Transform3D(Basis.Identity, new Vector3(0f, 1f, 0f));
|
||
_stage.AddChild(MakeStatic(new Vector3(3f, 1f, 3f), new Vector3(0f, 0.5f, 1f), 29));
|
||
}
|
||
|
||
private float _dropEyeErr, _dropMinPelvis = float.MaxValue;
|
||
|
||
private void TickDrop(float dt)
|
||
{
|
||
_stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
|
||
if (_ragdoll.Built && !_needBuild && _t > 0.5f)
|
||
{
|
||
_dropEyeErr = Mathf.Max(_dropEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
if (_ragdoll.Grounded && _ragdoll.GroundY < 0.5f)
|
||
_dropMinPelvis = Mathf.Min(_dropMinPelvis, _ragdoll.Pelvis.GlobalPosition.Y);
|
||
}
|
||
}
|
||
|
||
private bool FinishDrop()
|
||
{
|
||
GD.Print($"RAGDOLL: landing: eye vs camera max {_dropEyeErr:F3} m (limit 0.15), lowest pelvis after touchdown {_dropMinPelvis:F3} m (standing 0.99, limit > 0.7)");
|
||
bool landed = _dropEyeErr < 0.15f && _dropMinPelvis > 0.7f;
|
||
_dropEyeErr = 0f;
|
||
_dropMinPelvis = float.MaxValue;
|
||
if (!landed)
|
||
return false;
|
||
GD.Print($"RAGDOLL: drop: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis {_ragdoll.Pelvis.GlobalPosition}, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && Mathf.Abs(_ragdoll.GroundY) < 0.05f && Mathf.Abs(_origin.Origin.Y) < 0.1f
|
||
&& _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;
|
||
}
|
||
|
||
private float _poseDrift, _poseEyeErr;
|
||
|
||
// Six held poses, 3 s each (1 s blend + 2 s hold): the view rides the body, so any pose the body
|
||
// can't match would slowly walk the play space away — that must not happen.
|
||
private void TickPoseHold(float dt)
|
||
{
|
||
_measureFrom = float.MaxValue;
|
||
(float eye, float pitch, float fwd, Vector3 l, Vector3 r)[] poses =
|
||
{
|
||
(EyeHeight, 0f, 0f, new(-0.2f, 1.0f, -0.3f), new(0.2f, 1.0f, -0.3f)),
|
||
(1.0f, -0.3f, -0.1f, new(-0.2f, 0.4f, -0.35f), new(0.2f, 0.4f, -0.35f)),
|
||
(0.7f, -0.3f, -0.1f, new(-0.2f, 0.2f, -0.4f), new(0.2f, 0.2f, -0.4f)),
|
||
(1.35f, -0.6f, -0.35f, new(-0.2f, 0.9f, -0.6f), new(0.2f, 0.9f, -0.6f)),
|
||
(EyeHeight, 0f, 0f, new(-0.2f, 1.45f, -0.9f), new(0.25f, 1.6f, -0.85f)),
|
||
(0.9f, 0f, 0.05f, new(-0.25f, 0.6f, -0.2f), new(0.25f, 0.6f, -0.2f)),
|
||
};
|
||
int i = Mathf.Min((int)(_t / 3f), poses.Length - 1);
|
||
int prev = Mathf.Max(i - 1, 0);
|
||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((_t - i * 3f) / 1f, 0f, 1f));
|
||
var a = poses[prev];
|
||
var b = poses[i];
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, Mathf.Lerp(a.pitch, b.pitch, k)), new Vector3(0f, Mathf.Lerp(a.eye, b.eye, k), Mathf.Lerp(a.fwd, b.fwd, k)));
|
||
_leftLocal = new Transform3D(Basis.Identity, a.l.Lerp(b.l, k));
|
||
_rightLocal = new Transform3D(Basis.Identity, a.r.Lerp(b.r, k));
|
||
if (_ragdoll.Built && !_needBuild && _t - i * 3f > 2.5f)
|
||
_poseEyeErr = Mathf.Max(_poseEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
_poseDrift = Mathf.Max(_poseDrift, new Vector2(_origin.Origin.X, _origin.Origin.Z).Length());
|
||
}
|
||
|
||
private bool FinishPoseHold()
|
||
{
|
||
GD.Print($"RAGDOLL: pose hold: max play-space drift {_poseDrift:F3} m (limit 0.05), eye vs camera at rest max {_poseEyeErr:F3} m (limit 0.08), snaps {NewSnaps}");
|
||
bool ok = _finite && NewSnaps == 0 && _poseDrift < 0.05f && _poseEyeErr < 0.08f;
|
||
_poseDrift = _poseEyeErr = 0f;
|
||
return ok;
|
||
}
|
||
|
||
private float _hangEyeErr;
|
||
|
||
// Grab the bar and just hang (controllers stay up): the view must stay on the character's eye.
|
||
private void TickHang(float dt)
|
||
{
|
||
TickPullUp(both: true);
|
||
float y = BarY - 0.03f;
|
||
Basis grip = Aim(Vector3.Up + Vector3.Forward * 0.2f);
|
||
_rightLocal = new Transform3D(grip, new Vector3(0.22f, y, BarHangZ + 0.03f));
|
||
_leftLocal = new Transform3D(grip, new Vector3(-0.22f, y, BarHangZ + 0.03f));
|
||
_headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight + 0.15f * Mathf.Sin(_t * 2f), 0.05f * Mathf.Sin(_t * 1.3f)));
|
||
if (_barGrabbed && _t > 1.5f)
|
||
_hangEyeErr = Mathf.Max(_hangEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
}
|
||
|
||
private bool FinishHang()
|
||
{
|
||
GD.Print($"RAGDOLL: hang: eye vs camera max {_hangEyeErr:F3} m (limit 0.08), climbing {_ragdoll.Climbing}, snaps {NewSnaps}");
|
||
_ragdoll.SetHeld(0, null);
|
||
_ragdoll.SetHeld(1, null);
|
||
bool ok = _finite && NewSnaps == 0 && _hangEyeErr < 0.08f;
|
||
_hangEyeErr = 0f;
|
||
return ok;
|
||
}
|
||
|
||
// Walk to the waist-high ledge, crouch a little with the hands on the edge close to the body,
|
||
// then stand up while pressing the hands down, lean in, walk on.
|
||
private void TickHumanMantle(bool both)
|
||
{
|
||
float t = _t;
|
||
_stickVelocity = t < 1f || (t > 4.5f && t < 5.5f) ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero; // stop before the ledge's far end
|
||
float crouch = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((t - 1f) / 0.8f, 0f, 1f));
|
||
float rise = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((t - 2.2f) / 1.6f, 0f, 1f));
|
||
float eye = EyeHeight - 0.25f * crouch + 0.35f * rise;
|
||
float lean = 0.25f * rise;
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * crouch), new Vector3(0f, eye, -lean));
|
||
float handZ = ObstacleFront - 0.08f - _origin.Origin.Z;
|
||
float handY = t < 1f ? 1.0f : Mathf.Lerp(0.95f, 0.55f, rise) - 0.05f * crouch;
|
||
Basis down = Aim(Vector3.Down + Vector3.Forward * 0.3f);
|
||
_leftLocal = both ? new Transform3D(down, new Vector3(-0.2f, handY, handZ)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
|
||
_rightLocal = new Transform3D(down, new Vector3(0.2f, handY, handZ));
|
||
if (_ragdoll.Built && !_needBuild)
|
||
_maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - 0.99f);
|
||
if (t > 4.5f)
|
||
{
|
||
_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));
|
||
_headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
|
||
}
|
||
}
|
||
|
||
private const float ClimbFace = -0.4f;
|
||
private StaticBody3D _wall;
|
||
private readonly ConeTwistJoint3D[] _climbWeld = new ConeTwistJoint3D[2];
|
||
private readonly Vector3[] _gripWorld = new Vector3[2];
|
||
private float _climbStartPelvis, _climbEyeErr;
|
||
|
||
private void SetupClimbWall()
|
||
{
|
||
Rest();
|
||
_wall = MakeStatic(new Vector3(3f, 4f, 0.5f), new Vector3(0f, 2f, ClimbFace - 0.25f), 29);
|
||
_stage.AddChild(_wall);
|
||
_climbWeld[0] = _climbWeld[1] = null;
|
||
_gripWorld[0] = new Vector3(-0.2f, 1.75f, ClimbFace + 0.05f);
|
||
_gripWorld[1] = new Vector3(0.2f, 1.75f, ClimbFace + 0.05f);
|
||
_climbEyeErr = 0f;
|
||
}
|
||
|
||
private void Grip(int side, bool on)
|
||
{
|
||
if (on && _climbWeld[side] == null)
|
||
{
|
||
RigidBody3D hand = _ragdoll.Hand(side);
|
||
_climbWeld[side] = new ConeTwistJoint3D { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _wall.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
|
||
_stage.AddChild(_climbWeld[side]);
|
||
_ragdoll.SetHeld(side, _wall);
|
||
}
|
||
else if (!on && _climbWeld[side] != null)
|
||
{
|
||
_climbWeld[side].QueueFree();
|
||
_climbWeld[side] = null;
|
||
_ragdoll.SetHeld(side, null);
|
||
}
|
||
}
|
||
|
||
// Hold targets live in the world (the holds); while gripping, the player pulls the controllers
|
||
// down in the room. Free hands reach for the next hold 45 cm higher.
|
||
private readonly Vector3[] _gripLocal = new Vector3[2];
|
||
private readonly float[] _pullAtGrip = new float[2];
|
||
private int _climbStep; // 0 reach+grip both, 1 pull, 2 right moves up, 3 left moves up, 4 pull, 5 done
|
||
private float _climbStepT, _pulled;
|
||
|
||
// Like a climber: grip two holds, pull (hands go down ~45 cm relative to the BODY/room — the
|
||
// play space rides up with the body), move one hand to a higher hold and only then the other,
|
||
// pull again.
|
||
private void TickClimbWall(float dt)
|
||
{
|
||
float t = _t;
|
||
_measureFrom = float.MaxValue;
|
||
if (!_ragdoll.Built || _needBuild)
|
||
return;
|
||
if (t < dt * 1.5f)
|
||
{
|
||
_climbStep = 0;
|
||
_climbStepT = 0f;
|
||
_pulled = 0f;
|
||
}
|
||
_climbStepT += dt;
|
||
bool pulling = _climbStep == 1 || _climbStep == 4;
|
||
if (pulling)
|
||
_pulled = Mathf.MoveToward(_pulled, _climbStep == 1 ? 0.45f : 0.9f, 0.4f * dt);
|
||
Basis aim = Aim(Vector3.Up + Vector3.Forward * 0.15f);
|
||
for (int side = 0; side < 2; side++)
|
||
{
|
||
Vector3 local;
|
||
if (_climbWeld[side] != null)
|
||
local = _gripLocal[side] - Vector3.Up * (_pulled - _pullAtGrip[side]);
|
||
else
|
||
{
|
||
Vector3 current = side == 0 ? _leftLocal.Origin : _rightLocal.Origin;
|
||
local = current.MoveToward(_gripWorld[side] - _origin.Origin, 1.2f * dt);
|
||
if (_ragdoll.Hand(side).GlobalPosition.DistanceTo(_gripWorld[side]) < 0.08f && (_climbStep == 0 || _climbStep == 2 + (1 - side)))
|
||
{
|
||
Grip(side, true);
|
||
_gripLocal[side] = local;
|
||
_pullAtGrip[side] = _pulled;
|
||
}
|
||
}
|
||
if (side == 0)
|
||
_leftLocal = new Transform3D(aim, local);
|
||
else
|
||
_rightLocal = new Transform3D(aim, local);
|
||
}
|
||
switch (_climbStep)
|
||
{
|
||
case 0 when _climbWeld[0] != null && _climbWeld[1] != null:
|
||
_climbStartPelvis = _ragdoll.Pelvis.GlobalPosition.Y;
|
||
Next(1);
|
||
break;
|
||
case 1 when _pulled >= 0.45f:
|
||
MoveUp(1);
|
||
Next(2);
|
||
break;
|
||
case 2 when _climbWeld[1] != null && _climbStepT > 0.2f:
|
||
MoveUp(0);
|
||
Next(3);
|
||
break;
|
||
case 3 when _climbWeld[0] != null && _climbStepT > 0.2f:
|
||
Next(4);
|
||
break;
|
||
case 4 when _pulled >= 0.9f:
|
||
Next(5);
|
||
break;
|
||
}
|
||
if (t > 1f)
|
||
_climbEyeErr = Mathf.Max(_climbEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
|
||
void Next(int step)
|
||
{
|
||
_climbStep = step;
|
||
_climbStepT = 0f;
|
||
}
|
||
|
||
void MoveUp(int side)
|
||
{
|
||
Grip(side, false);
|
||
// Next hold: 30 cm above the hand that keeps holding.
|
||
_gripWorld[side] = _ragdoll.Hand(1 - side).GlobalPosition + Vector3.Up * 0.3f + Vector3.Right * (side == 0 ? -0.4f : 0.4f);
|
||
_gripWorld[side].Z = ClimbFace + 0.05f;
|
||
}
|
||
}
|
||
|
||
private bool FinishClimbWall()
|
||
{
|
||
float rise = _ragdoll.Pelvis.GlobalPosition.Y - _climbStartPelvis;
|
||
GD.Print($"RAGDOLL: wall climb: reached step {_climbStep}/5, pelvis rose {rise:F3} m (> 0.35), holding {_climbWeld[0] != null}/{_climbWeld[1] != null}, eye vs camera max {_climbEyeErr:F3} (< {PlayerRagdoll.MaxViewLead + 0.03f:F2}), snaps {NewSnaps}");
|
||
Grip(0, false);
|
||
Grip(1, false);
|
||
// MaxViewLead 0.08 is enforced before each step; this is measured after it (+ one tick of motion).
|
||
// Two 45 cm pulls minus the sag while hanging one-handed during the two hand moves.
|
||
return _finite && NewSnaps == 0 && _climbStep == 5 && rise > 0.35f && _climbEyeErr < PlayerRagdoll.MaxViewLead + 0.03f;
|
||
}
|
||
|
||
private RigidBody3D _seesaw;
|
||
|
||
// A 3 m plank along Z hinged 0.45 m up at z = −1; the player stands on its near half.
|
||
private void SetupSeesaw()
|
||
{
|
||
Rest();
|
||
StaticBody3D pivot = MakeStatic(new Vector3(0.6f, 0.45f, 0.3f), new Vector3(0f, 0.225f, -1f), 29);
|
||
_stage.AddChild(pivot);
|
||
_seesaw = MakeBox(new Vector3(0.6f, 0.06f, 3f), 12f, new Vector3(0f, 0.48f, -1f));
|
||
HingeJoint3D hinge = new() { Transform = new Transform3D(new Basis(Vector3.Back, Vector3.Up, Vector3.Left), new Vector3(0f, 0.48f, -1f)), NodeA = pivot.GetPath(), NodeB = _seesaw.GetPath() };
|
||
_stage.AddChild(hinge);
|
||
_origin = new Transform3D(Basis.Identity, new Vector3(0f, 0.51f, -0.2f)); // standing on the plank, 0.8 m from the pivot
|
||
}
|
||
|
||
private bool FinishSeesaw()
|
||
{
|
||
float tilt = Mathf.RadToDeg(_seesaw.GlobalTransform.Basis.Z.AngleTo(Vector3.Back));
|
||
GD.Print($"RAGDOLL: seesaw tilted {tilt:F1}° under the player (> 8), player feet y {_ragdoll.GroundY:F2}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && tilt > 8f;
|
||
}
|
||
|
||
private Playground _playground;
|
||
|
||
private void SetupPlayground()
|
||
{
|
||
Rest();
|
||
_playground = new Playground { Position = new Vector3(0f, 0f, 40f) }; // far from the player
|
||
_stage.AddChild(_playground);
|
||
}
|
||
|
||
private bool FinishPlayground()
|
||
{
|
||
RigidBody3D board = _playground.GetNode<RigidBody3D>("SwingBoard"), seesaw = _playground.GetNode<RigidBody3D>("Seesaw"), plank = _playground.GetNode<RigidBody3D>("Plank0");
|
||
Vector3 plankEnd = plank.GlobalTransform * new Vector3(0.9f, 0.05f, 0f);
|
||
PhysicsBody3D grabbed = PropGrabber.BodyUnderPalm(plankEnd, plank.GetWorld3D());
|
||
GD.Print($"RAGDOLL: playground: swing board y {board.GlobalPosition.Y:F2} (hangs ~1.75), seesaw y {seesaw.GlobalPosition.Y:F2} (on pivot ~0.48), plank end grab finds {(grabbed == null ? "nothing" : grabbed.Name)}");
|
||
return board.GlobalPosition.Y > 1.5f && seesaw.GlobalPosition.Y > 0.35f && grabbed is RigidBody3D && grabbed.Name.ToString().StartsWith("Plank"); // planks are stacked
|
||
}
|
||
|
||
// A slope rising away (−Z) from ObstacleFront up to 1 m, then a platform.
|
||
private void SetupRamp(float degrees)
|
||
{
|
||
Rest();
|
||
float a = Mathf.DegToRad(degrees);
|
||
float len = 1f / Mathf.Sin(a);
|
||
float run = Mathf.Cos(a) * len;
|
||
_stage.AddChild(new StaticBody3D { CollisionMask = 29, Transform = new Transform3D(new Basis(Vector3.Right, a), new Vector3(0f, 0.5f - 0.05f / Mathf.Cos(a), ObstacleFront - run * 0.5f)) });
|
||
_stage.GetChild<StaticBody3D>(_stage.GetChildCount() - 1).AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(3f, 0.1f, len) } });
|
||
_stage.AddChild(MakeStatic(new Vector3(3f, 1f, 3f), new Vector3(0f, 0.5f, ObstacleFront - run - 1.5f), 29));
|
||
}
|
||
|
||
private bool FinishRamp(bool walkable)
|
||
{
|
||
GD.Print($"RAGDOLL: ramp: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis {_ragdoll.Pelvis.GlobalPosition}, snaps {NewSnaps}");
|
||
bool up = _ragdoll.GroundY > 0.6f;
|
||
// Stopped at the slope's foot: on the floor, view level, not flung away.
|
||
bool stopped = Mathf.Abs(_origin.Origin.Y) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Length() < 2.5f;
|
||
return _finite && NewSnaps == 0 && (walkable ? up : !up && stopped);
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|