1408 lines
71 KiB
C#
1408 lines
71 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 string[] _only; // `-- only=jump,fall`: run just the phases whose names contain one of these
|
||
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;
|
||
if (arg.StartsWith("only="))
|
||
_only = arg[5..].Split(',');
|
||
}
|
||
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(("pose filter: render-timed tracking moves evenly (no 18 Hz beat)", Rest, _ => { }, 0.05f, FinishPoseFilter));
|
||
_phases.Add(("elbow: a controller turned outward doesn't fold the elbow inward", Rest, _ => { }, 0.05f, FinishElbowOut));
|
||
_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(("two-handed mantle onto a chest-high 1.2 m ledge", () => SetupObstacle(1.2f), _ => TickHumanMantle(both: true), 7f, () => FinishMantle(true)));
|
||
_phases.Add(("one hand can't mantle the 1.2 m ledge (the hips must be lifted)", () => SetupObstacle(1.2f), _ => TickHumanMantle(both: false), 7f, () => FinishMantle(false)));
|
||
_phases.Add(("mantles onto the playground's heavy box (25 kg physics object, 0.7 m)", SetupCrateLedge, _ => TickHumanMantle(both: true), 8.5f, FinishCrateMantle));
|
||
_phases.Add(("walks a 15 cm plank swaying the hips without falling off", SetupPlank, TickPlank, 6f, FinishPlank));
|
||
_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: a dip the view follows, no bounce", () => SetupLedgeTop(1f), TickDrop, 4f, () => FinishDrop(1f)));
|
||
_phases.Add(("big fall (5 m): legs fold fully, then it recovers", () => SetupLedgeTop(5f), TickDrop, 5f, () => FinishDrop(5f)));
|
||
_phases.Add(("jump: a tap of A hops", Rest, t => TickJump(0f, 0.05f), 4f, () => FinishJump(0)));
|
||
_phases.Add(("jump: holding A to charge jumps much higher", Rest, t => TickJump(0f, 0.6f), 4.5f, () => FinishJump(1)));
|
||
_phases.Add(("jump: a tap from a real crouch beats a standing tap", Rest, t => TickJump(0.45f, 0.05f), 4.5f, () => FinishJump(2)));
|
||
_phases.Add(("looks down past vertical while crouched (body doesn't flip around)", Rest, TickLookDown, 5f, FinishLookDown));
|
||
_phases.Add(("stick crouch lowers the body and the view, and stands back up", Rest, TickStickCrouch, 6f, FinishStickCrouch));
|
||
_phases.Add(("soft grab: a 1 kg cube 6 cm off the palm is eased into the hand, then held", () => SetupSoftGrab(prop: true), TickSoftGrab, 3f, FinishSoftGrab));
|
||
_phases.Add(("soft grab: a bar 5 cm off the palm — the hand goes to it", () => SetupSoftGrab(prop: false), TickSoftGrab, 3f, FinishSoftGrab));
|
||
_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)));
|
||
if (_only != null)
|
||
_phases.RemoveAll(ph => ph.Name != "hinge sign" && !Array.Exists(_only, o => ph.Name.Contains(o)));
|
||
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;
|
||
if (_driver != null)
|
||
_driver.StickCrouch = 0f;
|
||
_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;
|
||
|
||
// Right arm reaching forward-down, controller yawed 0..90° outward: the elbow must stay on the
|
||
// outer side of the shoulder→wrist line (toward +X), never toward the other arm.
|
||
private static bool FinishElbowOut()
|
||
{
|
||
float worst = float.MaxValue;
|
||
for (int deg = 0; deg <= 90; deg += 15)
|
||
{
|
||
ArmSolver arm = new();
|
||
Vector3 shoulder = new(0.2f, 1.45f, 0f), target = new(0.25f, 1.05f, -0.4f);
|
||
Basis hand = new Basis(Vector3.Up, -Mathf.DegToRad(deg)) * new Basis(Vector3.Right, -0.6f);
|
||
for (int i = 0; i < 200; i++)
|
||
arm.Solve(shoulder, target, Basis.Identity, Vector3.Right, hand, 0.3f, 0.27f, 1f / 144f);
|
||
Vector3 dir = (arm.Wrist - shoulder).Normalized();
|
||
Vector3 mid = shoulder + dir * (arm.Elbow - shoulder).Dot(dir);
|
||
worst = Mathf.Min(worst, arm.Elbow.X - mid.X); // < 0 = elbow toward the midline
|
||
}
|
||
GD.Print($"RAGDOLL: elbow vs the arm line with the controller turned 0..90° outward: most inward {worst:F3} m (limit -0.04)");
|
||
return worst > -0.04f;
|
||
}
|
||
|
||
// A controller moving at 1 m/s, sampled once per 90 Hz frame (predicted for that frame's render
|
||
// time), read by 144 Hz ticks. Per-tick output steps should all be 1/144 m: timestamped by
|
||
// arrival they alternate (1 or 2 ticks late), timestamped by render time they don't.
|
||
private static bool FinishPoseFilter()
|
||
{
|
||
float Spread(bool stamped)
|
||
{
|
||
PoseFilter f = new();
|
||
float min = float.MaxValue, max = 0f;
|
||
Vector3 prev = Vector3.Zero;
|
||
for (int tick = 0; tick < 300; tick++)
|
||
{
|
||
double now = tick / 144.0;
|
||
long frame = (long)Math.Floor(now * 90.0); // the last frame delivered before this tick
|
||
double render = (frame + 1) / 90.0; // its predicted display time
|
||
Transform3D raw = new(Basis.Identity, new Vector3((float)render, 0f, 0f));
|
||
Vector3 p = f.Sample(raw, now, stamped ? render : null).Origin;
|
||
if (tick > 20)
|
||
{
|
||
float step = p.X - prev.X;
|
||
min = Mathf.Min(min, step);
|
||
max = Mathf.Max(max, step);
|
||
}
|
||
prev = p;
|
||
}
|
||
return (max - min) * 144f; // spread of the per-tick speed, m/s
|
||
}
|
||
float arrival = Spread(false), stamped = Spread(true);
|
||
GD.Print($"RAGDOLL: pose filter at 1 m/s: per-tick speed spread {arrival:F3} m/s by arrival, {stamped:F3} m/s render-timed");
|
||
return stamped < 0.05f && stamped < arrival;
|
||
}
|
||
|
||
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();
|
||
_ledgeBox = null;
|
||
_ledgeTop = height;
|
||
if (height >= 1f)
|
||
_origin = new Transform3D(Basis.Identity, new Vector3(0f, 0f, ObstacleFront + 0.4f));
|
||
_stage.AddChild(MakeStatic(new Vector3(3f, height, depth), new Vector3(0f, height * 0.5f, ObstacleFront - depth * 0.5f), 29));
|
||
}
|
||
|
||
// Playground.HeavyBox (25 kg, 0.7 m cube) as the ledge, same human mantle as onto the static ledge.
|
||
private void SetupCrateLedge()
|
||
{
|
||
Rest();
|
||
_ledgeBox = MakeBox(Vector3.One * 0.7f, 25f, new Vector3(0f, 0.35f, ObstacleFront - 0.35f));
|
||
_ledgeTop = 0.7f;
|
||
}
|
||
|
||
private RigidBody3D _ledgeBox;
|
||
private float _ledgeTop = 0.9f;
|
||
|
||
private bool FinishCrateMantle()
|
||
{
|
||
GD.Print($"RAGDOLL: crate mantle: pelvis rose up to {_maxPelvisRise:F3} m, feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
|
||
_maxPelvisRise = 0f;
|
||
return _finite && NewSnaps == 0 && _ragdoll.GroundY > 0.6f && _origin.Origin.Y > 0.55f;
|
||
}
|
||
|
||
private const float PlankY = 0.3f;
|
||
private float _plankMinFoot;
|
||
|
||
private void SetupPlank()
|
||
{
|
||
Rest();
|
||
_origin = new Transform3D(Basis.Identity, new Vector3(0f, PlankY, 0f));
|
||
_stage.AddChild(MakeStatic(new Vector3(0.15f, PlankY, 8f), new Vector3(0f, PlankY * 0.5f, -3f), 29));
|
||
_plankMinFoot = float.MaxValue;
|
||
}
|
||
|
||
// Walk along it with the head (and so the hips) swaying ±9 cm side to side.
|
||
private void TickPlank(float dt)
|
||
{
|
||
float sway = _t > 1f ? 0.09f * Mathf.Sin(Mathf.Tau * 0.7f * (_t - 1f)) : 0f;
|
||
_headLocal = new Transform3D(Basis.Identity, new Vector3(sway, EyeHeight, 0f));
|
||
_stickVelocity = _t > 1f && _t < 5f ? new Vector3(0f, 0f, -0.6f) : Vector3.Zero;
|
||
_measureFrom = float.MaxValue;
|
||
if (_ragdoll.Built && !_needBuild && _t > 0.5f)
|
||
_plankMinFoot = Mathf.Min(_plankMinFoot, _ragdoll.Grounded ? _ragdoll.GroundY : -1f);
|
||
}
|
||
|
||
private bool FinishPlank()
|
||
{
|
||
GD.Print($"RAGDOLL: plank: lowest supported foot level {_plankMinFoot:F3} (plank top {PlankY}), pelvis {_ragdoll.Pelvis.GlobalPosition}, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _plankMinFoot > PlankY - 0.03f && _ragdoll.Pelvis.GlobalPosition.Z < -1.8f;
|
||
}
|
||
|
||
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 > _ledgeTop - 0.05f && _origin.Origin.Y > _ledgeTop - 0.1f;
|
||
_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 platform whose edge is 0.5 m ahead.
|
||
private void SetupLedgeTop(float height)
|
||
{
|
||
Rest();
|
||
_origin = new Transform3D(Basis.Identity, new Vector3(0f, height, 0f));
|
||
_stage.AddChild(MakeStatic(new Vector3(3f, height, 3f), new Vector3(0f, height * 0.5f, 1f), 29));
|
||
_dropEyeErr = _dropEyeErrLate = _dropRebound = 0f;
|
||
_dropMinPelvis = float.MaxValue;
|
||
_dropStand = 0f;
|
||
}
|
||
|
||
private float _dropEyeErr, _dropEyeErrLate, _dropMinPelvis = float.MaxValue, _dropStand, _dropRebound;
|
||
|
||
private void TickDrop(float dt)
|
||
{
|
||
_stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
|
||
if (!_ragdoll.Built || _needBuild)
|
||
return;
|
||
float pelvis = _ragdoll.Pelvis.GlobalPosition.Y;
|
||
if (_t < 0.5f)
|
||
_dropStand = pelvis - _ragdoll.GroundY; // standing hips above the feet
|
||
else
|
||
{
|
||
_dropEyeErr = Mathf.Max(_dropEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
if (_ragdoll.Grounded && _ragdoll.GroundY < 0.5f)
|
||
{
|
||
if (pelvis < _dropMinPelvis)
|
||
{
|
||
_dropMinPelvis = pelvis;
|
||
_dropRebound = pelvis;
|
||
}
|
||
_dropRebound = Mathf.Max(_dropRebound, pelvis);
|
||
}
|
||
if (_dropMinPelvis < float.MaxValue && _ragdoll.Pelvis.LinearVelocity.Y > -0.5f)
|
||
_dropEyeErrLate = Mathf.Max(_dropEyeErrLate, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
}
|
||
}
|
||
|
||
private bool FinishDrop(float height)
|
||
{
|
||
float dip = _dropStand - _dropMinPelvis, overshoot = _dropRebound - _dropStand;
|
||
// The dip grows with the impact; a big fall folds the legs (hips near the ground). Rebound
|
||
// past the stance would be a bounce. The view rides the body throughout.
|
||
bool dipOk = height < 2f ? dip > 0.12f && dip < 0.5f : dip > 0.45f;
|
||
GD.Print($"RAGDOLL: landing from {height} m: dip {dip:F3} m (stance {_dropStand:F3}), rebound above the stance {overshoot:F3} (limit 0.05), eye vs camera max {_dropEyeErr:F3} / after the impact {_dropEyeErrLate:F3} (limit 0.1)");
|
||
bool landed = dipOk && overshoot < 0.05f && _dropEyeErrLate < 0.1f;
|
||
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
|
||
&& Mathf.Abs(_ragdoll.Pelvis.GlobalPosition.Y - _ragdoll.GroundY - _dropStand) < 0.05f;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- soft grab
|
||
|
||
private PhysicsBody3D _sgTarget;
|
||
private SoftGrab _sg;
|
||
private ConeTwistJoint3D _sgWeld;
|
||
private float _sgGapAtWeld, _sgMaxSpeed, _sgHeldGap, _sgWeldTime;
|
||
private bool _sgProp;
|
||
|
||
private void SetupSoftGrab(bool prop)
|
||
{
|
||
Rest();
|
||
_sgProp = prop;
|
||
_sg = null;
|
||
_sgWeld = null;
|
||
_sgGapAtWeld = _sgHeldGap = -1f;
|
||
_sgMaxSpeed = 0f;
|
||
_sgWeldTime = -1f;
|
||
_sgTarget = null;
|
||
}
|
||
|
||
// Right controller held still in front; at 1.5 s the target appears off the palm and grip is pressed.
|
||
private void TickSoftGrab(float dt)
|
||
{
|
||
_measureFrom = float.MaxValue;
|
||
if (!_ragdoll.Built || _needBuild)
|
||
return;
|
||
RigidBody3D hand = _ragdoll.Hand(1);
|
||
if (_sgTarget == null && _t >= 1.5f)
|
||
{
|
||
Vector3 n = SoftGrab.PalmNormal(hand, 1);
|
||
Vector3 surface = SoftGrab.PalmSurface(hand, 1);
|
||
if (_sgProp)
|
||
{
|
||
RigidBody3D cube = MakeBox(Vector3.One * 0.08f, 1f, surface + n * (0.06f + 0.04f));
|
||
cube.GravityScale = 0f; // "floating" just for the setup instant; gravity back on at the weld
|
||
_sgTarget = cube;
|
||
}
|
||
else
|
||
{
|
||
StaticBody3D bar = new() { CollisionMask = 29, Position = surface + n * (0.05f + 0.025f) };
|
||
bar.AddChild(new CollisionShape3D { Shape = new CylinderShape3D { Radius = 0.025f, Height = 1f } });
|
||
_stage.AddChild(bar);
|
||
_sgTarget = bar;
|
||
}
|
||
return; // let the new body enter the space before querying it
|
||
}
|
||
if (_sgTarget == null)
|
||
return;
|
||
if (_sg == null && _sgWeld == null)
|
||
_sg = SoftGrab.Begin(hand, 1, _sgTarget);
|
||
if (_sg != null)
|
||
{
|
||
if (_sgTarget is RigidBody3D rb)
|
||
_sgMaxSpeed = Mathf.Max(_sgMaxSpeed, rb.LinearVelocity.Length());
|
||
_sgMaxSpeed = Mathf.Max(_sgMaxSpeed, hand.LinearVelocity.Length());
|
||
if (_sg.Step(dt))
|
||
{
|
||
_sgGapAtWeld = _sg.Gap;
|
||
_sgWeldTime = _t;
|
||
_sg = null;
|
||
_sgWeld = new ConeTwistJoint3D { Position = PropGrabber.PalmPoint(hand, 1), NodeA = hand.GetPath(), NodeB = _sgTarget.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f, ExcludeNodesFromCollision = true };
|
||
_stage.AddChild(_sgWeld);
|
||
if (_sgTarget is RigidBody3D cube)
|
||
cube.GravityScale = 1f;
|
||
_ragdoll.SetHeld(1, _sgTarget);
|
||
}
|
||
}
|
||
}
|
||
|
||
private bool FinishSoftGrab()
|
||
{
|
||
float held = _sgTarget == null ? -1f : (SoftGrab.PalmSurface(_ragdoll.Hand(1), 1) - _sgTarget.GlobalPosition).Length() - (_sgProp ? 0.04f : 0.025f);
|
||
GD.Print($"RAGDOLL: soft grab ({(_sgProp ? "1 kg cube" : "static bar")}): welded after {(_sgWeldTime - 1.5f):F3} s with gap {_sgGapAtWeld:F4} m, fastest motion {_sgMaxSpeed:F2} m/s, held gap at the end {held:F3} m, snaps {NewSnaps}");
|
||
_ragdoll.SetHeld(1, null);
|
||
return _finite && NewSnaps == 0 && _sgWeldTime > 0f && _sgWeldTime - 1.5f < 0.25f && _sgGapAtWeld < (_sgProp ? 0.01f : 0.015f) && _sgMaxSpeed < 3.5f && held < 0.02f;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- jump
|
||
|
||
private float _jumpStand, _jumpApex, _jumpEyeErr, _tapApex;
|
||
private bool _jumpPressed;
|
||
|
||
// Optionally squat (eyes `crouch` lower) first, then hold A from 2 s for `hold` s; the player keeps the pose.
|
||
private void TickJump(float crouch, float hold)
|
||
{
|
||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((_t - 0.8f) / 0.8f, 0f, 1f)) * (crouch > 0f ? 1f : 0f);
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.2f * k), new Vector3(0f, EyeHeight - crouch * k, -0.05f * k));
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f - crouch * k, -0.3f));
|
||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f - crouch * k, -0.3f));
|
||
_measureFrom = float.MaxValue;
|
||
if (!_ragdoll.Built || _needBuild)
|
||
return;
|
||
float pelvis = _ragdoll.Pelvis.GlobalPosition.Y;
|
||
if (_t < 0.7f)
|
||
{
|
||
_jumpStand = pelvis;
|
||
_jumpApex = 0f;
|
||
_jumpEyeErr = 0f;
|
||
_jumpPressed = false;
|
||
}
|
||
_ragdoll.JumpHeld = _t >= 2f && _t < 2f + hold;
|
||
if (_t >= 2f)
|
||
_jumpPressed = true;
|
||
if (_jumpPressed)
|
||
{
|
||
_jumpApex = Mathf.Max(_jumpApex, pelvis - _jumpStand);
|
||
_jumpEyeErr = Mathf.Max(_jumpEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
}
|
||
}
|
||
|
||
private bool FinishJump(int kind)
|
||
{
|
||
string what = kind == 0 ? "standing tap" : kind == 1 ? "full charge" : "tap from a crouch";
|
||
GD.Print($"RAGDOLL: {what}: hips rose {_jumpApex:F3} m above the standing stance, eye vs camera max {_jumpEyeErr:F3}, grounded {_ragdoll.Grounded}, jumping {_ragdoll.Jumping}, snaps {NewSnaps}");
|
||
bool ok = _finite && NewSnaps == 0 && _ragdoll.Grounded && !_ragdoll.Jumping && _jumpEyeErr < 0.12f && _origin.Origin.Length() < 0.15f;
|
||
if (kind == 0)
|
||
{
|
||
_tapApex = _jumpApex;
|
||
return ok && _jumpApex > 0.03f && _jumpApex < 0.35f;
|
||
}
|
||
if (kind == 1)
|
||
return ok && _jumpApex > _tapApex + 0.25f && _jumpApex < 0.9f;
|
||
return ok && _jumpApex > _tapApex + 0.1f;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- looking down past vertical
|
||
|
||
private float _lookYaw0, _lookYawMax, _lookChestMax, _lookPitchMax;
|
||
|
||
private static float YawOfBody(RigidBody3D b)
|
||
{
|
||
Vector3 f = BodyMath.HeadFacing(b.GlobalTransform.Basis.Orthonormalized());
|
||
return f == Vector3.Zero ? 0f : BodyMath.YawOf(f);
|
||
}
|
||
|
||
// Crouch (eyes 1.0 m), then look down to 125° below the horizon (chin on the chest, looking
|
||
// behind the feet) and back, hands down near the floor — interacting with stuff on the ground.
|
||
private void TickLookDown(float dt)
|
||
{
|
||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1f, 1f));
|
||
float pitch = Mathf.DegToRad(125f) * Mathf.Sin(Mathf.Pi * Mathf.Clamp((_t - 1.5f) / 3f, 0f, 1f));
|
||
_lookPitchMax = Mathf.Max(_lookPitchMax, pitch);
|
||
Basis head = new(Vector3.Right, -pitch);
|
||
Vector3 pivot = new(0f, Mathf.Lerp(EyeHeight, 1.0f, k) + TrueBodySolver.EyeToNeckPivot.Y, -0.1f * k + TrueBodySolver.EyeToNeckPivot.Z);
|
||
_headLocal = new Transform3D(head, pivot - head * TrueBodySolver.EyeToNeckPivot);
|
||
_leftLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f));
|
||
_rightLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f));
|
||
_measureFrom = float.MaxValue;
|
||
if (!_ragdoll.Built || _needBuild)
|
||
return;
|
||
if (_t < 1.4f)
|
||
{
|
||
_lookYaw0 = YawOfBody(_ragdoll.Pelvis);
|
||
_lookYawMax = _lookChestMax = _lookPitchMax = 0f;
|
||
return;
|
||
}
|
||
_lookYawMax = Mathf.Max(_lookYawMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Pelvis))));
|
||
_lookChestMax = Mathf.Max(_lookChestMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Chest))));
|
||
}
|
||
|
||
private bool FinishLookDown()
|
||
{
|
||
GD.Print($"RAGDOLL: look down to {Mathf.RadToDeg(_lookPitchMax):F0}°: pelvis yaw change max {Mathf.RadToDeg(_lookYawMax):F1}°, chest {Mathf.RadToDeg(_lookChestMax):F1}° (limit 15), eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _lookYawMax < Mathf.DegToRad(15f) && _lookChestMax < Mathf.DegToRad(15f)
|
||
&& _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.08f;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- stick crouch
|
||
|
||
private float _stickEyeLow, _stickOriginLow, _stickEyeErr;
|
||
|
||
private void TickStickCrouch(float dt)
|
||
{
|
||
_measureFrom = float.MaxValue;
|
||
if (_driver == null)
|
||
return;
|
||
// Hold the stick down for 1 s (1.2 m/s → capped at 0.6 m here), hold, then back up.
|
||
float c = _t < 0.5f ? 0f : _t < 3.5f ? Mathf.Min(0.6f, (_t - 0.5f) * 1.2f) : Mathf.Max(0f, 0.6f - (_t - 3.5f) * 1.2f);
|
||
_driver.StickCrouch = c;
|
||
if (_t > 2.5f && _t < 3.4f && _ragdoll.Built)
|
||
{
|
||
_stickEyeLow = _ragdoll.Eye.Y;
|
||
_stickOriginLow = _origin.Origin.Y;
|
||
_stickEyeErr = Mathf.Max(_stickEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
}
|
||
}
|
||
|
||
private bool FinishStickCrouch()
|
||
{
|
||
GD.Print($"RAGDOLL: stick crouch 0.6 m: eye y {_stickEyeLow:F3} (standing {EyeHeight}), origin y {_stickOriginLow:F3}, eye vs camera {_stickEyeErr:F3}; back up: eye y {_ragdoll.Eye.Y:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
|
||
bool ok = _finite && NewSnaps == 0 && Mathf.Abs(_stickEyeLow - (EyeHeight - 0.6f)) < 0.1f && Mathf.Abs(_stickOriginLow + 0.6f) < 0.08f
|
||
&& _stickEyeErr < 0.06f && Mathf.Abs(_ragdoll.Eye.Y - EyeHeight) < 0.06f && Mathf.Abs(_origin.Origin.Y) < 0.06f;
|
||
_stickEyeErr = 0f;
|
||
return ok;
|
||
}
|
||
|
||
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;
|
||
// Walk up to a waist-high ledge (stop before its far end); a chest-high one starts within reach.
|
||
// (A 70 cm box is too small to walk on after climbing it.)
|
||
_stickVelocity = (t < 1f && _ledgeTop < 1f) || (t > 4.5f && t < 5.5f && _ledgeBox == null) ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero;
|
||
// Crouch to load the legs for a waist-high ledge; at a chest-high one you reach up instead.
|
||
float crouch = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((t - 1f) / 0.8f, 0f, 1f)) * (_ledgeTop < 1f ? 1f : 0f);
|
||
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 = (_ledgeTop < 1f ? 0.25f : 0.55f) * rise; // over a chest-high edge you put your head well over it
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * crouch), new Vector3(0f, eye, -lean));
|
||
// Hands on the top just past the near edge (of the box itself when it's a movable one).
|
||
float front = _ledgeBox != null ? _ledgeBox.GlobalPosition.Z + 0.35f : ObstacleFront;
|
||
float handZ = front - 0.08f - _origin.Origin.Z;
|
||
float handY = t < 1f ? 1.0f : Mathf.Lerp(_ledgeTop + 0.05f, _ledgeTop - 0.35f, rise) - 0.05f * crouch - _origin.Origin.Y;
|
||
Basis down = Aim(Vector3.Down + Vector3.Forward * 0.3f);
|
||
// Walking up, the hands are down at the sides; then they go up over the edge onto the top.
|
||
float reach = Mathf.Clamp((t - 0.8f) / 0.4f, 0f, 1f);
|
||
Vector3 rest = new(0.2f, 1.0f, -0.3f), onTop = new(0.2f, Mathf.Max(handY, _ledgeTop + 0.08f - _origin.Origin.Y), handZ);
|
||
Vector3 r = reach < 1f ? rest.Lerp(new Vector3(0.2f, _ledgeTop + 0.12f - _origin.Origin.Y, -0.3f), reach) : new Vector3(0.2f, handY, handZ);
|
||
if (reach >= 1f && t < 1.4f)
|
||
r = onTop;
|
||
_leftLocal = both ? new Transform3D(down, new Vector3(-r.X, r.Y, r.Z)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
|
||
_rightLocal = new Transform3D(down, r);
|
||
if (_ragdoll.Built && !_needBuild)
|
||
_maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - 0.99f);
|
||
if (OS.HasEnvironment("MANTLETRACE") && _ragdoll.Built && ((int)(t * 144f)) % 14 == 0)
|
||
GD.Print($"MT t {t:F2} pelvis {_ragdoll.Pelvis.GlobalPosition} v {_ragdoll.Pelvis.LinearVelocity.Y:F2} o {_origin.Origin.Y:F2} j {_ragdoll.Jumping} braced {_ragdoll.Braced} lifted {_ragdoll.Lifted} legs {_ragdoll.LegsSolid} g {_ragdoll.Grounded} foot {_ragdoll.GroundY:F2} hands {_ragdoll.Hand(0).GlobalPosition.Y:F2}/{_ragdoll.Hand(1).GlobalPosition.Y:F2} {(_stage.GetChildCount() > 0 && _stage.GetChild(0) is RigidBody3D rb ? rb.GlobalPosition.ToString() : "")}");
|
||
if (t > (_ledgeTop < 1f ? 4.5f : 5.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));
|
||
// The free hand reaching for the next hold while hanging on the other (tipped body): it
|
||
// should stay on its controller (owner: "tracking off when the body is tilted").
|
||
for (int side = 0; side < 2; side++)
|
||
if (_climbWeld[side] == null && _climbStep is 2 or 3 && _ragdoll.Hand(side).GetContactCount() == 0)
|
||
{
|
||
Vector3 ctrl = (side == 0 ? Left : Right).Origin;
|
||
_climbFreeErr += _ragdoll.Hand(side).GlobalPosition.DistanceTo(ctrl);
|
||
_climbFreeN++;
|
||
}
|
||
|
||
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 float _climbFreeErr;
|
||
private int _climbFreeN;
|
||
|
||
private bool FinishClimbWall()
|
||
{
|
||
float rise = _ragdoll.Pelvis.GlobalPosition.Y - _climbStartPelvis;
|
||
GD.Print($"RAGDOLL: wall climb: free hand vs its controller while reaching {(_climbFreeN > 0 ? _climbFreeErr / _climbFreeN : 0f):F3} m mean");
|
||
_climbFreeErr = 0f;
|
||
_climbFreeN = 0;
|
||
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 = GD.Load<PackedScene>("res://scenes/maps/playground.tscn").Instantiate<Playground>(); // the saved scene
|
||
_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;
|
||
}
|
||
}
|