Body step 1: one-time T-pose calibration (prefs.json, forced gate) + True Body solver + shared ArmSolver
- BodyProfile (Winter anthropometry, T-pose: arm = (span - shoulder width)/2 - 1.5 cm) and Prefs (gitignored prefs.json next to the executable / project folder in editor builds) - Calibration node: loads prefs, else head-locked prompt + jitter-proof TPoseDetector; body systems wait for BodyProfile.Current; Recalibrate() in group "calibration"; recal rebuilds arms - ArmSolver: PhysicsArms' elbow solve extracted (behaviour-identical) + min reach |U-F| fix - TrueBody: derived real-body pose (neck, chest yaw + crouch lean at neck pivot, lagging pelvis, moving shoulder girdle with scapulohumeral shrug, arms) + debug skeleton - PhysicsArms/PhysicsTorso/BodyRig now profile-driven (arm length no longer from head height) - TrueBodySpike PASS x3; all spikes PASS x3 (arm spike thresholds re-based for correct 0.57 m arms; CollapseSpike uses a fixed standing profile) Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
1 parent
3a990c6dbe
commit
81a9b1fca2
15 files changed
+994
-106
No files matched your search
@@ -0,0 +1,192 @@
|
||||
using Godot;
|
||||
|
||||
// Detects a held T-pose from head + wrist positions in XR-origin space (floor at y = 0) and
|
||||
// measures it. Pure logic so headless spikes can feed it synthetic poses.
|
||||
public sealed class TPoseDetector
|
||||
{
|
||||
public const float HoldTime = 1.0f;
|
||||
public const float StillRadius = 0.03f; // every point must stay within this of where the hold began (jitter-proof)
|
||||
private const float LevelTolerance = 0.08f;
|
||||
private const float SymmetryTolerance = 0.10f;
|
||||
private const float MaxLineVsForward = 0.35f;
|
||||
private const float MinSpan = 1.0f;
|
||||
private const float MaxSpan = 2.4f;
|
||||
private const float MinDropBelowEye = 0.05f;
|
||||
private const float MaxDropBelowEye = 0.5f;
|
||||
|
||||
private Vector3 _anchorHead, _anchorLeft, _anchorRight;
|
||||
private float _held;
|
||||
private float _sumEye, _sumSpan, _sumWristY;
|
||||
private int _samples;
|
||||
|
||||
public string Problem { get; private set; } = "";
|
||||
public float Progress => Mathf.Clamp(_held / HoldTime, 0f, 1f);
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_held = 0f;
|
||||
_sumEye = _sumSpan = _sumWristY = 0f;
|
||||
_samples = 0;
|
||||
Problem = "";
|
||||
}
|
||||
|
||||
// Returns a profile once the pose has been valid and still for HoldTime.
|
||||
public BodyProfile Feed(Vector3 head, Vector3 headForward, Vector3 wristL, Vector3 wristR, float dt)
|
||||
{
|
||||
Problem = Validate(head, headForward, wristL, wristR);
|
||||
bool still = true;
|
||||
if (Problem.Length == 0 && _samples > 0)
|
||||
still = head.DistanceTo(_anchorHead) < StillRadius && wristL.DistanceTo(_anchorLeft) < StillRadius
|
||||
&& wristR.DistanceTo(_anchorRight) < StillRadius;
|
||||
if (Problem.Length == 0 && !still)
|
||||
Problem = "hold still";
|
||||
if (Problem.Length > 0)
|
||||
{
|
||||
_held = 0f;
|
||||
_sumEye = _sumSpan = _sumWristY = 0f;
|
||||
_samples = 0;
|
||||
return null;
|
||||
}
|
||||
|
||||
if (_samples == 0)
|
||||
{
|
||||
_anchorHead = head;
|
||||
_anchorLeft = wristL;
|
||||
_anchorRight = wristR;
|
||||
}
|
||||
_held += dt;
|
||||
_sumEye += head.Y;
|
||||
_sumSpan += wristL.DistanceTo(wristR);
|
||||
_sumWristY += (wristL.Y + wristR.Y) * 0.5f;
|
||||
_samples++;
|
||||
if (_held < HoldTime)
|
||||
return null;
|
||||
|
||||
BodyProfile profile = BodyProfile.FromTPose(_sumEye / _samples, _sumSpan / _samples, _sumWristY / _samples);
|
||||
if (!profile.IsPlausible())
|
||||
{
|
||||
Problem = "measurement implausible — try again";
|
||||
Reset();
|
||||
return null;
|
||||
}
|
||||
return profile;
|
||||
}
|
||||
|
||||
private static string Validate(Vector3 head, Vector3 headForward, Vector3 wristL, Vector3 wristR)
|
||||
{
|
||||
if (Mathf.Abs(wristL.Y - wristR.Y) > LevelTolerance)
|
||||
return "level your arms";
|
||||
float drop = head.Y - (wristL.Y + wristR.Y) * 0.5f;
|
||||
if (drop < MinDropBelowEye || drop > MaxDropBelowEye)
|
||||
return "arms out at shoulder height";
|
||||
float span = wristL.DistanceTo(wristR);
|
||||
if (span < MinSpan || span > MaxSpan)
|
||||
return "stretch both arms fully out";
|
||||
Vector2 headXZ = new(head.X, head.Z);
|
||||
float dl = headXZ.DistanceTo(new Vector2(wristL.X, wristL.Z));
|
||||
float dr = headXZ.DistanceTo(new Vector2(wristR.X, wristR.Z));
|
||||
if (Mathf.Abs(dl - dr) > SymmetryTolerance)
|
||||
return "arms out evenly to both sides";
|
||||
Vector3 line = wristR - wristL;
|
||||
line.Y = 0f;
|
||||
Vector3 fwd = new(headForward.X, 0f, headForward.Z);
|
||||
if (line.LengthSquared() > 1e-6f && fwd.LengthSquared() > 1e-6f
|
||||
&& Mathf.Abs(line.Normalized().Dot(fwd.Normalized())) > MaxLineVsForward)
|
||||
return "face forward, arms to the sides";
|
||||
return "";
|
||||
}
|
||||
}
|
||||
|
||||
// One-time body calibration gate. On launch: load prefs.json; if there is no valid profile the
|
||||
// body systems wait (they check BodyProfile.Current) and a prompt asks for a T-pose. Recalibrate()
|
||||
// (menu button; group "calibration") repeats it at any time.
|
||||
public partial class Calibration : Node
|
||||
{
|
||||
public const float WristOffset = 0.06f; // wrist sits behind the aim-pose origin along its +Z
|
||||
|
||||
private XRCamera3D _camera;
|
||||
private XRController3D _left;
|
||||
private XRController3D _right;
|
||||
private Label3D _prompt;
|
||||
private readonly TPoseDetector _detector = new();
|
||||
private bool _active;
|
||||
private float _doneTimer;
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
AddToGroup("calibration");
|
||||
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
||||
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||||
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
||||
_prompt = new Label3D
|
||||
{
|
||||
FontSize = 48,
|
||||
PixelSize = 0.0012f,
|
||||
OutlineSize = 10,
|
||||
NoDepthTest = true,
|
||||
Visible = false,
|
||||
Position = new Vector3(0f, -0.05f, -1.1f),
|
||||
};
|
||||
_camera.AddChild(_prompt);
|
||||
|
||||
BodyProfile saved = Prefs.LoadBody();
|
||||
if (saved != null)
|
||||
{
|
||||
BodyProfile.Set(saved);
|
||||
GD.Print($"Calibration: loaded {Prefs.FilePath}: {saved}");
|
||||
}
|
||||
else
|
||||
{
|
||||
GD.Print($"Calibration: no valid profile at {Prefs.FilePath} — T-pose calibration required");
|
||||
Begin();
|
||||
}
|
||||
}
|
||||
|
||||
public void Recalibrate()
|
||||
{
|
||||
GD.Print("Calibration: recalibration requested");
|
||||
Begin();
|
||||
}
|
||||
|
||||
private void Begin()
|
||||
{
|
||||
_active = true;
|
||||
_doneTimer = 0f;
|
||||
_detector.Reset();
|
||||
_prompt.Visible = true;
|
||||
}
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
float dt = (float)delta;
|
||||
if (_doneTimer > 0f)
|
||||
{
|
||||
_doneTimer -= dt;
|
||||
if (_doneTimer <= 0f)
|
||||
_prompt.Visible = false;
|
||||
}
|
||||
if (!_active)
|
||||
return;
|
||||
|
||||
// Origin-local: the floor is y = 0 and world locomotion doesn't matter.
|
||||
Vector3 head = _camera.Position;
|
||||
Vector3 forward = _camera.Transform.Basis * Vector3.Forward;
|
||||
Vector3 wristL = _left.Transform * new Vector3(0f, 0f, WristOffset);
|
||||
Vector3 wristR = _right.Transform * new Vector3(0f, 0f, WristOffset);
|
||||
BodyProfile result = _detector.Feed(head, forward, wristL, wristR, dt);
|
||||
|
||||
if (result == null)
|
||||
{
|
||||
string hint = _detector.Problem.Length > 0 ? _detector.Problem : $"hold… {Mathf.RoundToInt(_detector.Progress * 100f)}%";
|
||||
_prompt.Text = $"Body calibration\nStand straight, arms out to the sides (T-pose)\n{hint}";
|
||||
return;
|
||||
}
|
||||
|
||||
_active = false;
|
||||
BodyProfile.Set(result);
|
||||
bool saved = Prefs.SaveBody(result);
|
||||
_prompt.Text = $"Calibrated ✓\narm {result.ArmLength * 100f:F1} cm · height {result.Stature * 100f:F0} cm";
|
||||
_doneTimer = 2.5f;
|
||||
GD.Print($"Calibration: T-pose measured: {result}; saved {(saved ? Prefs.FilePath : "FAILED")}");
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user