FoxyzandClaude Opus 5.5
d83273dd2f
PlayerRagdoll: 9 bodies, spine/neck ball joints, shoulder drives (girdle + muscle), elbow hinges, torque-capped motor PDs toward the True Body, chest->hand guides, implicit balance motor, mass-scaled ground control. PlayerRig: XR driver with stick move / snap turn and Bonelab-style push-back (eye->camera sphere sweep). Replaces the chase bodies in main.tscn; RagdollSpike regression (14 phases). Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
477 lines
21 KiB
C#
477 lines
21 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;
|
||
private Vector3 _prevL, _prevR;
|
||
private bool _needBuild;
|
||
|
||
// Measurements (reset per phase).
|
||
private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
|
||
private bool _finite;
|
||
private int _snapsAtStart;
|
||
private float _measureFrom;
|
||
|
||
private Transform3D Camera => _origin * _headLocal;
|
||
private Transform3D Left => _origin * _leftLocal;
|
||
private Transform3D Right => _origin * _rightLocal;
|
||
|
||
public override void _Ready()
|
||
{
|
||
_profile = BodyProfile.Estimate(EyeHeight);
|
||
AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29));
|
||
_ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
|
||
AddChild(_ragdoll);
|
||
|
||
_phases.Add(("hinge sign", SetupHinge, TickHinge, 0.3f, FinishHinge));
|
||
_phases.Add(("stands still, head on the camera, hands on the controllers", Rest, _ => { }, 4f, FinishStand));
|
||
_phases.Add(("reaches forward, out and overhead", Rest, TickReach, 4.5f, FinishReach));
|
||
_phases.Add(("fast arm swings stay attached", Rest, TickSwing, 3f, FinishSwing));
|
||
_phases.Add(("stick-walking into a wall is stopped (view pushed back)", SetupWall, _ => _stickVelocity = new Vector3(0f, 0f, -2f), 3f, FinishWall));
|
||
_phases.Add(("room-scale walking into a wall is stopped", SetupWall, TickRoomWalk, 3f, FinishWall));
|
||
_phases.Add(("standing walk into a low bar is blocked", SetupBar, _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishBarBlocked));
|
||
_phases.Add(("crouched walk passes under the low bar", SetupBar, TickCrouchWalk, 4f, FinishBarPassed));
|
||
_phases.Add(("leans over a table without being pushed back", SetupTable, TickLean, 3f, FinishLean));
|
||
_phases.Add(("pushes a 10 kg crate", SetupCrate, TickPush, 3f, FinishCrate));
|
||
_phases.Add(("12 kg held close to the body", () => SetupWeight(close: true), _ => { }, 3f, () => FinishWeight(close: true)));
|
||
_phases.Add(("12 kg held at arm's length sags more", () => SetupWeight(close: false), _ => { }, 3f, () => FinishWeight(close: false)));
|
||
_phases.Add(("teleport + snap turn carry the body", Rest, TickTeleport, 2f, FinishTeleport));
|
||
_phases.Add(("random motion soak", Rest, TickRandom, 8f, FinishSoak));
|
||
Next();
|
||
}
|
||
|
||
// ---------------------------------------------------------------- harness
|
||
|
||
public override void _PhysicsProcess(double delta)
|
||
{
|
||
if (_phase >= _phases.Count)
|
||
return;
|
||
float dt = (float)delta;
|
||
_t += dt;
|
||
_phases[_phase].Tick(dt);
|
||
if (_phase > 0)
|
||
Drive(dt);
|
||
if (_t >= _phases[_phase].Duration)
|
||
{
|
||
bool ok = _phases[_phase].Finish();
|
||
_allOk &= ok;
|
||
GD.Print($"RAGDOLL: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}");
|
||
Next();
|
||
}
|
||
}
|
||
|
||
private void Next()
|
||
{
|
||
_stage?.QueueFree();
|
||
_stage = new Node3D();
|
||
AddChild(_stage);
|
||
_t = 0f;
|
||
_phase++;
|
||
if (_phase >= _phases.Count)
|
||
{
|
||
GD.Print($"RAGDOLL: overall {(_allOk ? "PASS — muscle ragdoll player functional" : "CHECK — inspect failures above")}");
|
||
GetTree().Quit();
|
||
return;
|
||
}
|
||
_maxEyeErr = _maxHandErr = _endHandErr = _maxSpeed = _maxElbowErr = 0f;
|
||
_minCamZ = _minEyeZ = float.MaxValue;
|
||
_finite = true;
|
||
_measureFrom = 1f;
|
||
_stickVelocity = Vector3.Zero;
|
||
_phases[_phase].Setup();
|
||
_snapsAtStart = _ragdoll.Snaps;
|
||
}
|
||
|
||
// Exactly PlayerRig's tick order.
|
||
private void Drive(float dt)
|
||
{
|
||
_origin.Origin += _stickVelocity * dt;
|
||
if (_ragdoll.Built && !_needBuild)
|
||
_origin.Origin += _ragdoll.OriginCorrection(Camera.Origin, _origin.Origin.Y, dt);
|
||
_solver.Solve(_profile, Camera, Left, Right, _origin.Origin.Y, dt);
|
||
if (_needBuild)
|
||
{
|
||
_ragdoll.Build(_profile, _solver, Left, Right);
|
||
_needBuild = false;
|
||
_prevL = Left.Origin;
|
||
_prevR = Right.Origin;
|
||
}
|
||
Vector3 lv = (Left.Origin - _prevL) / dt, rv = (Right.Origin - _prevR) / dt;
|
||
_prevL = Left.Origin;
|
||
_prevR = Right.Origin;
|
||
_ragdoll.Step(_solver, Left, Right, lv, rv, _origin.Origin.Y, dt);
|
||
Measure();
|
||
}
|
||
|
||
private void Measure()
|
||
{
|
||
foreach (RigidBody3D b in _ragdoll.Bodies)
|
||
_finite &= BodyMath.IsFinite(b);
|
||
_minCamZ = Mathf.Min(_minCamZ, Camera.Origin.Z);
|
||
_minEyeZ = Mathf.Min(_minEyeZ, _ragdoll.Eye.Z);
|
||
if (_t < _measureFrom)
|
||
return;
|
||
_maxEyeErr = Mathf.Max(_maxEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||
float handErr = Mathf.Max(_ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin));
|
||
_maxHandErr = Mathf.Max(_maxHandErr, handErr);
|
||
_endHandErr = handErr;
|
||
float speed = 0f;
|
||
foreach (RigidBody3D b in _ragdoll.Bodies)
|
||
speed = Mathf.Max(speed, b.LinearVelocity.Length());
|
||
_maxSpeed = speed;
|
||
for (int side = 0; side < 2; side++)
|
||
{
|
||
Vector3 up = _solver.Elbow[side] - _solver.Shoulder[side], fore = _solver.Wrist[side] - _solver.Elbow[side];
|
||
float target = up.AngleTo(fore);
|
||
_maxElbowErr = Mathf.Max(_maxElbowErr, Mathf.Abs(target - _ragdoll.ElbowFlex(side)));
|
||
}
|
||
}
|
||
|
||
|
||
private int NewSnaps => _ragdoll.Snaps - _snapsAtStart;
|
||
|
||
private static StaticBody3D MakeStatic(Vector3 size, Vector3 pos, uint mask = 0)
|
||
{
|
||
StaticBody3D body = new() { Position = pos, CollisionMask = mask };
|
||
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
|
||
return body;
|
||
}
|
||
|
||
private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos)
|
||
{
|
||
RigidBody3D body = new() { Mass = mass, Position = pos, CollisionMask = 29 };
|
||
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
|
||
_stage.AddChild(body);
|
||
return body;
|
||
}
|
||
|
||
// Standing, looking ahead, hands in front at waist height (elbows ~90°), controllers pointing forward.
|
||
private void Rest()
|
||
{
|
||
_origin = Transform3D.Identity;
|
||
_headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
|
||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f, -0.3f));
|
||
_solver.Reset();
|
||
_needBuild = true;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- hinge sign
|
||
|
||
private RigidBody3D _hingeBody;
|
||
private HingeJoint3D _hinge;
|
||
private Basis _hingeStart;
|
||
|
||
private void SetupHinge()
|
||
{
|
||
StaticBody3D anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0f, 5f, 0f));
|
||
_stage.AddChild(anchor);
|
||
_hingeBody = new RigidBody3D { Position = new Vector3(0f, 5f, 0f), GravityScale = 0f };
|
||
_hingeBody.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.4f, 0.1f, 0.1f) } });
|
||
_stage.AddChild(_hingeBody);
|
||
_hinge = new HingeJoint3D { Position = new Vector3(0f, 5f, 0f), NodeA = anchor.GetPath(), NodeB = _hingeBody.GetPath() };
|
||
_hinge.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
|
||
_hinge.SetParam(HingeJoint3D.Param.MotorTargetVelocity, 1f);
|
||
_hinge.SetParam(HingeJoint3D.Param.MotorMaxImpulse, 50f);
|
||
_stage.AddChild(_hinge);
|
||
_hingeStart = _hingeBody.GlobalTransform.Basis;
|
||
}
|
||
|
||
private void TickHinge(float dt) { }
|
||
|
||
private bool FinishHinge()
|
||
{
|
||
// Joint +Z = world +Z here. Positive motor velocity rotated the body about ±Z?
|
||
Vector3 x = _hingeBody.GlobalTransform.Basis.X;
|
||
float about = _hingeStart.X.Cross(x).Z;
|
||
PlayerRagdoll.HingeSign = about >= 0f ? 1f : -1f;
|
||
GD.Print($"RAGDOLL: +motor velocity turns the child about {(about >= 0 ? "+" : "-")}Z → HingeSign {PlayerRagdoll.HingeSign}");
|
||
return Mathf.Abs(about) > 0.05f;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- phases
|
||
|
||
private bool FinishStand()
|
||
{
|
||
GD.Print($"RAGDOLL: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3} m, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, end speed {_maxSpeed:F3} m/s, grounded {_ragdoll.Grounded}, snaps {NewSnaps}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F3}");
|
||
return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && _maxSpeed < 0.05f && _maxElbowErr < Mathf.DegToRad(8f);
|
||
}
|
||
|
||
private readonly List<string> _reachLog = new();
|
||
private bool _reachOk = true;
|
||
|
||
private void TickReach(float dt)
|
||
{
|
||
_measureFrom = float.MaxValue;
|
||
float reach = _profile.ArmLength;
|
||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||
(Vector3 l, Vector3 r)[] poses =
|
||
{
|
||
(new Vector3(-0.2f, sh, -reach - 0.05f), new Vector3(0.2f, sh, -reach - 0.05f)), // forward, straight
|
||
(new Vector3(-_profile.ShoulderHalfWidth - reach, sh, 0f), new Vector3(_profile.ShoulderHalfWidth + reach, sh, 0f)), // T
|
||
(new Vector3(-0.2f, sh + reach, -0.05f), new Vector3(0.2f, sh + reach, -0.05f)), // overhead
|
||
};
|
||
int i = Mathf.Min((int)(_t / 1.5f), poses.Length - 1);
|
||
float local = _t - i * 1.5f;
|
||
// Controllers point along the arm, as a real hand does at full reach.
|
||
Vector3 shL = new(-_profile.ShoulderHalfWidth, sh, 0.06f), shR = new(_profile.ShoulderHalfWidth, sh, 0.06f);
|
||
_leftLocal = new Transform3D(Aim(poses[i].l - shL), poses[i].l);
|
||
_rightLocal = new Transform3D(Aim(poses[i].r - shR), poses[i].r);
|
||
if (local > 1.4f && local - dt <= 1.4f)
|
||
{
|
||
// Unreachable by up to the girdle travel: the hand stops at the arm's end, not inside the body.
|
||
float errL = _ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), errR = _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin);
|
||
float e = Mathf.Max(errL, errR);
|
||
_reachLog.Add($"pose {i}: hand error {e:F3} m");
|
||
_reachOk &= e < 0.06f;
|
||
}
|
||
}
|
||
|
||
// Controller basis whose aim (−Z) points along `dir`, thumb up where possible.
|
||
private static Basis Aim(Vector3 dir)
|
||
{
|
||
Vector3 z = -dir.Normalized();
|
||
Vector3 up = Mathf.Abs(z.Y) > 0.9f ? Vector3.Back : Vector3.Up;
|
||
Vector3 x = up.Cross(z).Normalized();
|
||
return new Basis(x, z.Cross(x), z);
|
||
}
|
||
|
||
private bool FinishReach()
|
||
{
|
||
GD.Print($"RAGDOLL: {string.Join(", ", _reachLog)}; snaps {NewSnaps}");
|
||
bool ok = _finite && NewSnaps == 0 && _reachOk && _reachLog.Count == 3;
|
||
_reachLog.Clear();
|
||
_reachOk = true;
|
||
return ok;
|
||
}
|
||
|
||
private void TickSwing(float dt)
|
||
{
|
||
float a = Mathf.Sin(_t * Mathf.Tau * 1.5f);
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.25f, 1.15f + 0.25f * a, -0.35f - 0.15f * a));
|
||
_rightLocal = new Transform3D(new Basis(Vector3.Right, 0.8f * a), new Vector3(0.25f + 0.2f * a, 1.15f - 0.25f * a, -0.35f));
|
||
}
|
||
|
||
private bool FinishSwing()
|
||
{
|
||
GD.Print($"RAGDOLL: hand lag max {_maxHandErr:F3} m (limit 0.15), eye error max {_maxEyeErr:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _maxHandErr < 0.15f && _maxEyeErr < 0.08f;
|
||
}
|
||
|
||
private const float WallFace = -1.0f;
|
||
|
||
private void SetupWall()
|
||
{
|
||
Rest();
|
||
_stage.AddChild(MakeStatic(new Vector3(4f, 3f, 0.4f), new Vector3(0f, 1.5f, WallFace - 0.2f), 29));
|
||
}
|
||
|
||
private void TickRoomWalk(float dt)
|
||
{
|
||
Vector3 step = new(0f, 0f, -1f * dt);
|
||
_headLocal.Origin += step;
|
||
_leftLocal.Origin += step;
|
||
_rightLocal.Origin += step;
|
||
}
|
||
|
||
private bool FinishWall()
|
||
{
|
||
GD.Print($"RAGDOLL: wall face z {WallFace}: camera min z {_minCamZ:F3}, eye min z {_minEyeZ:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _minCamZ > WallFace - 0.02f && _minEyeZ > WallFace - 0.02f && _minCamZ < WallFace + 0.5f;
|
||
}
|
||
|
||
private const float BarZ = -0.8f;
|
||
|
||
private void SetupBar()
|
||
{
|
||
Rest();
|
||
// A beam across the path whose underside is at 1.2 m.
|
||
_stage.AddChild(MakeStatic(new Vector3(4f, 0.1f, 0.1f), new Vector3(0f, 1.25f, BarZ), 29));
|
||
}
|
||
|
||
private bool FinishBarBlocked()
|
||
{
|
||
// The chest hits the beam; the head may lean over it, but the view can't run off ahead of the body.
|
||
GD.Print($"RAGDOLL: bar at z {BarZ}: eye min z {_minEyeZ:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _minEyeZ > BarZ - 0.25f && _minCamZ > _minEyeZ - PlayerRagdoll.FreeSlack - 0.02f;
|
||
}
|
||
|
||
private void TickCrouchWalk(float dt)
|
||
{
|
||
// Crouch over the first second (eyes to 1.0 m, looking slightly down), then walk forward.
|
||
float k = Mathf.Min(_t / 1f, 1f);
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.0f, k), 0f));
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
|
||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
|
||
_stickVelocity = _t > 1.2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
|
||
}
|
||
|
||
private bool FinishBarPassed()
|
||
{
|
||
GD.Print($"RAGDOLL: crouched: camera min z {_minCamZ:F3} (bar at {BarZ}), eye {_ragdoll.Eye.Y:F2} m, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _minCamZ < BarZ - 0.6f;
|
||
}
|
||
|
||
private void SetupTable()
|
||
{
|
||
Rest();
|
||
_stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, -0.85f), 29));
|
||
}
|
||
|
||
private void TickLean(float dt)
|
||
{
|
||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
|
||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.6f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.3f, k), Mathf.Lerp(0f, -0.4f, k)));
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
|
||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
|
||
_measureFrom = 2f;
|
||
}
|
||
|
||
private bool FinishLean()
|
||
{
|
||
GD.Print($"RAGDOLL: eye error after the lean max {_maxEyeErr:F3} m (limit 0.08), hand error {_endHandErr:F3}, origin drift {_origin.Origin.Length():F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _origin.Origin.Length() < 0.05f;
|
||
}
|
||
|
||
private RigidBody3D _crate;
|
||
private Vector3 _crateStart;
|
||
|
||
private void SetupCrate()
|
||
{
|
||
Rest();
|
||
_crate = MakeBox(new Vector3(0.4f, 1.4f, 0.4f), 10f, new Vector3(0f, 0.7f, -0.55f)); // within reach at chest height
|
||
_crateStart = _crate.Position;
|
||
}
|
||
|
||
private void TickPush(float dt)
|
||
{
|
||
float k = Mathf.Clamp((_t - 0.5f) / 2f, 0f, 1f);
|
||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
|
||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
|
||
}
|
||
|
||
private bool FinishCrate()
|
||
{
|
||
float moved = _crateStart.Z - _crate.GlobalPosition.Z;
|
||
GD.Print($"RAGDOLL: crate pushed {moved:F3} m (front face now z {_crate.GlobalPosition.Z + 0.2f:F3}), hands z {_ragdoll.Hand(0).GlobalPosition.Z:F3}/{_ragdoll.Hand(1).GlobalPosition.Z:F3} y {_ragdoll.Hand(0).GlobalPosition.Y:F3}, elbows {Mathf.RadToDeg(_ragdoll.ElbowFlex(0)):F0}°, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && moved > 0.15f;
|
||
}
|
||
|
||
private RigidBody3D _weight;
|
||
private float _closeSag = -1f;
|
||
|
||
private void SetupWeight(bool close)
|
||
{
|
||
Rest();
|
||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||
_rightLocal = close
|
||
? new Transform3D(Basis.Identity, new Vector3(0.18f, sh - 0.1f, -0.25f))
|
||
: new Transform3D(Basis.Identity, new Vector3(_profile.ShoulderHalfWidth, sh, -_profile.ArmLength * 0.95f));
|
||
_measureFrom = 2f;
|
||
// Build now so the weight can be welded to the placed hand.
|
||
Drive(1f / 90f);
|
||
RigidBody3D hand = _ragdoll.Hand(1);
|
||
Vector3 at = hand.GlobalTransform * new Vector3(0f, 0f, -0.12f); // just beyond the fingers
|
||
_weight = MakeBox(new Vector3(0.12f, 0.12f, 0.12f), 12f, at);
|
||
_weight.GlobalPosition = at;
|
||
_ragdoll.SetHeld(1, _weight);
|
||
ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _weight.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
|
||
_stage.AddChild(weld);
|
||
}
|
||
|
||
private bool FinishWeight(bool close)
|
||
{
|
||
float sag = Right.Origin.Y - _ragdoll.Hand(1).GlobalPosition.Y;
|
||
GD.Print($"RAGDOLL: right hand sag {sag:F3} m under 12 kg ({(close ? "close" : "far")}), snaps {NewSnaps}");
|
||
if (close)
|
||
{
|
||
_closeSag = sag;
|
||
return _finite && NewSnaps == 0 && sag < 0.08f;
|
||
}
|
||
return _finite && NewSnaps == 0 && sag > _closeSag + 0.03f;
|
||
}
|
||
|
||
private bool _teleported, _turned;
|
||
private float _teleportEyeErr;
|
||
|
||
private void TickTeleport(float dt)
|
||
{
|
||
_measureFrom = 1.2f;
|
||
if (!_teleported && _t > 0.5f)
|
||
{
|
||
Transform3D before = _origin;
|
||
_origin = new Transform3D(new Basis(Vector3.Up, Mathf.Pi / 2f), new Vector3(5f, 0f, 3f));
|
||
Transform3D delta = _origin * before.AffineInverse();
|
||
_ragdoll.TeleportBy(delta);
|
||
_solver.Shift(delta);
|
||
_prevL = delta * _prevL;
|
||
_prevR = delta * _prevR;
|
||
_teleported = true;
|
||
}
|
||
if (!_turned && _t > 0.9f)
|
||
{
|
||
Vector3 pivot = Camera.Origin;
|
||
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, Mathf.DegToRad(30f)), Vector3.Zero) * new Transform3D(Basis.Identity, -pivot);
|
||
_origin = rot * _origin;
|
||
_ragdoll.TeleportBy(rot);
|
||
_solver.Shift(rot);
|
||
_prevL = rot * _prevL;
|
||
_prevR = rot * _prevR;
|
||
_turned = true;
|
||
}
|
||
}
|
||
|
||
private bool FinishTeleport()
|
||
{
|
||
GD.Print($"RAGDOLL: after teleport + snap turn: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3}, snaps {NewSnaps}");
|
||
_teleported = _turned = false;
|
||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.05f && _maxHandErr < 0.06f;
|
||
}
|
||
|
||
private readonly RandomNumberGenerator _rng = new() { Seed = 11 };
|
||
private Vector3 _ra, _rb, _rc;
|
||
|
||
private void TickRandom(float dt)
|
||
{
|
||
if (_t < dt * 1.5f)
|
||
{
|
||
_ra = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||
_rb = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||
_rc = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||
}
|
||
Vector3 W(Vector3 ph, float f) => new(Mathf.Sin(_t * f + ph.X), Mathf.Sin(_t * f * 1.3f + ph.Y), Mathf.Sin(_t * f * 0.7f + ph.Z));
|
||
Vector3 h = W(_ra, 1.1f);
|
||
_headLocal = new Transform3D(Basis.FromEuler(new Vector3(0.4f * h.X, 0.9f * h.Y, 0.2f * h.Z)), new Vector3(0.1f * h.Z, EyeHeight - 0.15f + 0.1f * h.X, 0.1f * h.Y));
|
||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||
Vector3 l = W(_rb, 2.3f), r = W(_rc, 2.0f);
|
||
_leftLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * l.Y, 0.8f * l.Z, 1.0f * l.X)), new Vector3(-0.25f + 0.15f * l.X, sh - 0.25f + 0.2f * l.Y, -0.3f + 0.15f * l.Z));
|
||
_rightLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * r.Y, 0.8f * r.Z, 1.0f * r.X)), new Vector3(0.25f + 0.15f * r.X, sh - 0.25f + 0.2f * r.Y, -0.3f + 0.15f * r.Z));
|
||
}
|
||
|
||
private bool FinishSoak()
|
||
{
|
||
GD.Print($"RAGDOLL: random motion: eye error max {_maxEyeErr:F3}, hand error max {_maxHandErr:F3}, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, snaps {NewSnaps}");
|
||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _maxHandErr < 0.12f;
|
||
}
|
||
}
|