From 81a9b1fca2c6b6095e9e22412e85e9dbf2c852d1 Mon Sep 17 00:00:00 2001 From: Foxyz Date: Fri, 2 Oct 2026 05:47:13 +0300 Subject: [PATCH] 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 --- .gitignore | 2 + AGENTS.md | 9 ++ HANDOFF.md | 33 ++++- scenes/main.tscn | 10 +- scenes/truebody_spike.tscn | 6 + scripts/ArmSolver.cs | 102 +++++++++++++++ scripts/ArmSpike.cs | 6 +- scripts/BodyProfile.cs | 132 ++++++++++++++++++++ scripts/BodyRig.cs | 8 +- scripts/Calibration.cs | 192 ++++++++++++++++++++++++++++ scripts/CollapseSpike.cs | 3 + scripts/PhysicsArms.cs | 135 ++++++-------------- scripts/PhysicsTorso.cs | 11 +- scripts/TrueBody.cs | 247 +++++++++++++++++++++++++++++++++++++ scripts/TrueBodySpike.cs | 204 ++++++++++++++++++++++++++++++ 15 files changed, 994 insertions(+), 106 deletions(-) create mode 100644 scenes/truebody_spike.tscn create mode 100644 scripts/ArmSolver.cs create mode 100644 scripts/BodyProfile.cs create mode 100644 scripts/Calibration.cs create mode 100644 scripts/TrueBody.cs create mode 100644 scripts/TrueBodySpike.cs diff --git a/.gitignore b/.gitignore index 0b6d2a4..4450cb5 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ .vendor/venv/ .export/ *.translation +prefs.json +.claude/worktrees/ diff --git a/AGENTS.md b/AGENTS.md index eaa97f9..5a9050e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -270,6 +270,15 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x the head/neck look override exists. **HANDOFF.md at project root is the takeover document** (constant table verified against code — code wins on drift); keep it updated when systems change. +11. **Body rebuild toward Nimso's Project TX (`research/nimso-project-tx.md`)** — step 0 DONE (fork 6-DOF joints, + `research/godot-box3d-6dof.md`); step 1 DONE 2026-10-02: one-time T-pose CALIBRATION persisted in a + gitignored `prefs.json` next to the executable (forced gate on launch, `Recalibrate()` via group + "calibration"), `BodyProfile` (Winter anthropometry, arm stored 1.5 cm short), shared `ArmSolver`, and the + TRUE BODY solver (`TrueBody.cs`: neck, chest yaw + crouch lean, lagging pelvis, moving shoulder girdle, arms) + with a debug skeleton. Arm-length decisions: `research/vr-avatar-arm-length.md`. Next: step 2 = the + muscle-driven ragdoll player (ball joints + torque-capped motors toward the True Body), replacing the + chase-body chest/arms/hands; ground control; then legs. Owner scope: thumbstick locomotion only, no jump, + avatar scaling and NPCs later. 10. **Body physics Stage 2 — PHYSICAL ARMS, implemented 2026-10-02, awaiting owner playtest.** `PhysicsArms.cs`: per side an upper-arm + forearm RigidBody3D (2.0 / 1.2 kg, layer 5) that PD-chase their analytic 2-bone IK pose, solved each physics tick from the PHYSICAL chest diff --git a/HANDOFF.md b/HANDOFF.md index 914380a..577fa1c 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -38,8 +38,8 @@ TERMINOLOGY (owner-corrected — use precisely): collapse spike takes seconds instead of minutes). Spikes and their pass lines: `physics_spike` "SPIKE: overall PASS", `weld_spike` "WELD: …", `grab_spike` "GRAB: …", `body_spike` "BODY: …", `collapse_spike` "COLLAPSE: …", `arm_spike` "ARM: overall PASS", - `sixdof_spike` "6DOF: overall PASS" (run FIRST with `weld_spike` whenever the addon binary changes). - All seven PASS 3× as of this handoff. Headless asset import: + `sixdof_spike` "6DOF: overall PASS" (run FIRST with `weld_spike` whenever the addon binary changes), + `truebody_spike` "TRUEBODY: overall PASS" (no physics; plain `--headless`). All eight PASS 3× as of this handoff. Headless asset import: `godot-mono --headless --import`. - Engine logs: `~/.local/share/godot/app_userdata/VR Messs/logs/godot.log` — **read after user playtests instead of guessing**. @@ -182,6 +182,32 @@ TERMINOLOGY (owner-corrected — use precisely): connected (an obstructed segment shows as a displaced elbow). Hidden until PhysicsArms calibrates. IK, calibration and reach budget moved to PhysicsArms; torso mesh + yaw spring live in PhysicsTorso. +- **Calibration.cs + BodyProfile.cs (body step 1, 2026-10-02)** — ONE-TIME T-POSE CALIBRATION (owner design: + VRChat-FBT-like, persists, forced on launch if missing). `BodyProfile` = the player's real metrics (eye height, + stature = eye/0.936, shoulder half-width = 0.1295·stature, shoulder drop below the eyes, upper arm/forearm in + the 0.186:0.146 Winter split, stored 1.5 cm SHORT so the real elbow never locks first). `Prefs` stores it in + **`prefs.json` next to the executable (editor builds incl. headless: the project folder) — gitignored, + portable (copy the file)**. `Calibration` (node in group "calibration", `Recalibrate()` for the menu) loads it; + if missing it shows a head-locked Label3D and runs `TPoseDetector`: arms level (±8 cm), at shoulder height, + symmetric about the head (±10 cm), to the sides (not forward), span 1.0–2.4 m, every point within 3 cm of where + the hold began (jitter-proof) for 1 s → measure: arm = (wrist span − shoulder width)/2 − margin. Body systems + (PhysicsTorso, PhysicsArms, BodyRig, TrueBody) WAIT for `BodyProfile.Current`; recalibration rebuilds them. + Flat preview (no headset) can't calibrate → no body there (expected). Headless spikes that inject a head use + `BodyProfile.Estimate(eye)`; CollapseSpike sets a fixed standing profile (posture never shrinks the body). +- **ArmSolver.cs** — the shared analytic 2-bone solve (moved verbatim from PhysicsArms: soft IK 0.98, pole as an + angle about the arm axis vs torso-frame down/back/out, 85% controller-forward suggestion read nearest the current + angle, ±110° clamp, 6 rad/s, elbow height cap). NEW: minimum reach |upper − fore| + 1 cm (unequal anatomical + segments made the law of cosines invalid for hands right at the shoulder). +- **TrueBody.cs (body step 1)** — `TrueBodySolver`: Nimso's TRUE BODY — the derived pose of the real body from + head + controllers + profile (trigonometry, not IK): neck pivot 9 cm below / 7 cm behind the eyes (head frame); + chest yaw = the HMD/hands yaw spring; chest pitch = crouch lean (crouch measured at the NECK PIVOT so nodding + isn't crouching; up to ~43°) + ¼ of head pitch-down; neck base along chest-up/head-up average; pelvis hangs + 0.288·stature below the shoulder line through a critically damped 2.5 Hz spring (hip lag/sway, hips kick back in + a crouch); MOVING SHOULDER GIRDLE: past 98% reach the shoulder joint travels toward the hand within anatomical + limits (out +3/in −2, up +7/down −2, forward +8/back −3 cm) plus a scapulohumeral shrug up to 4 cm overhead; + arms via ArmSolver. `TrueBody` node solves every physics tick and draws a translucent cyan DEBUG SKELETON + (`TrueBody.ShowDebug`) for in-headset judgement. Step 2 (muscle ragdoll) will drive its joints toward this pose. + Spike `scenes/truebody_spike.tscn`. - **BodyMath.cs** — shared servo math. `RotationError(target, current)` = shortest-arc axis·angle via `2·atan2(|xyz|, W)` — **the only allowed way to compute servo rotation errors** (`Quaternion.GetAngle()` NaNs at W = 1.0000001, `GetAxis()` is un-normalized near @@ -402,6 +428,9 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29). ## 7. Next steps (in order) +0. **Owner playtest of body step 1** — T-pose calibration (forced on first launch; delete `prefs.json` to redo, + or the menu's Recalibrate), arm length should now match; judge the cyan True Body skeleton (shoulders that + shrug/reach, crouch lean, hip sway, elbows). Then body step 2 = the muscle ragdoll on the 6-DOF joints. 1. **Owner re-playtest of the body (chest fix + Stage 2 arms)** — the Stage-1 collapse is fixed (`collapse_spike` 49/49); feel is unvalidated in-headset. Read the log after. 2. **Locomotion decision** — owner report 2026-10-01: two-hand anchor turning broken (not diff --git a/scenes/main.tscn b/scenes/main.tscn index f0b8a15..34a3d8b 100644 --- a/scenes/main.tscn +++ b/scenes/main.tscn @@ -1,4 +1,4 @@ -[gd_scene load_steps=21 format=3] +[gd_scene load_steps=23 format=3] [ext_resource type="Script" path="res://scripts/Main.cs" id="1_main"] [ext_resource type="PackedScene" path="res://scenes/teleport_marker.tscn" id="2_marker"] @@ -11,6 +11,8 @@ [ext_resource type="Script" path="res://scripts/BodyRig.cs" id="9_bodyrig"] [ext_resource type="Script" path="res://scripts/PhysicsTorso.cs" id="10_physicsTorso"] [ext_resource type="Script" path="res://scripts/PhysicsArms.cs" id="11_physicsArms"] +[ext_resource type="Script" path="res://scripts/Calibration.cs" id="12_calibration"] +[ext_resource type="Script" path="res://scripts/TrueBody.cs" id="13_truebody"] [sub_resource type="ProceduralSkyMaterial" id="ProceduralSkyMaterial_sky"] sky_horizon_color = Color(0.66, 0.72, 0.81, 1) @@ -122,6 +124,12 @@ script = ExtResource("10_physicsTorso") [node name="PhysicsArms" type="Node" parent="."] script = ExtResource("11_physicsArms") +[node name="Calibration" type="Node" parent="."] +script = ExtResource("12_calibration") + +[node name="TrueBody" type="Node3D" parent="."] +script = ExtResource("13_truebody") + [node name="BodyRig" type="Node3D" parent="."] script = ExtResource("9_bodyrig") diff --git a/scenes/truebody_spike.tscn b/scenes/truebody_spike.tscn new file mode 100644 index 0000000..a11e8ee --- /dev/null +++ b/scenes/truebody_spike.tscn @@ -0,0 +1,6 @@ +[gd_scene load_steps=2 format=3] + +[ext_resource type="Script" path="res://scripts/TrueBodySpike.cs" id="1_spike"] + +[node name="TrueBodySpike" type="Node"] +script = ExtResource("1_spike") diff --git a/scripts/ArmSolver.cs b/scripts/ArmSolver.cs new file mode 100644 index 0000000..824bfe8 --- /dev/null +++ b/scripts/ArmSolver.cs @@ -0,0 +1,102 @@ +using Godot; + +// Analytic 2-bone arm solve shared by PhysicsArms and TrueBody (one instance per arm — it keeps +// the elbow's smoothed pole angle). Soft-IK clamp, then the elbow is picked on its circle as an +// ANGLE around the arm axis relative to a torso-frame down/back/out reference: mostly what the +// hand's forward axis suggests (the forearm continues behind the wrist), clamped to ±110° and +// rate-limited so it can never swing through the inward/up side (the 180° flip), and capped +// at max(shoulder, wrist) + 5 cm height (no chicken wings). +public sealed class ArmSolver +{ + public const float SoftIKFactor = 0.98f; + public const float PoleHandWeight = 0.85f; + public const float PoleSmoothRate = 12f; + public const float PoleMaxAngle = 1.92f; // 110 deg either way from down/back/out + public const float PoleMaxSpeed = 6f; // rad/s + public const float ElbowRiseAllowance = 0.05f; + + private float _poleAngle; + private Vector3 _lastRef; + + public Vector3 Elbow { get; private set; } + public Vector3 Wrist { get; private set; } + + // side = the arm's outward direction (−torso X for the left arm, +X for the right). + // handBasis = the controller's basis (null → torso-frame reference only). + public void Solve(Vector3 shoulder, Vector3 rawTarget, Basis torsoBasis, Vector3 side, Basis? handBasis, + float upperLen, float foreLen, float delta) + { + float reach = upperLen + foreLen; + Vector3 shoulderToTarget = rawTarget - shoulder; + if (shoulderToTarget.LengthSquared() < 0.0001f) + shoulderToTarget = torsoBasis * new Vector3(0f, -0.25f, -0.3f); + + float rawDist = shoulderToTarget.Length(); + // Below |upper − fore| the triangle can't close (law of cosines invalid) — with unequal + // anatomical segments a hand near the shoulder would otherwise throw the elbow away. + float d = Mathf.Clamp(rawDist, Mathf.Abs(upperLen - foreLen) + 0.01f, reach * SoftIKFactor); + Vector3 dir = shoulderToTarget / rawDist; + Vector3 wrist = shoulder + dir * d; + + // Elbow circle: center on the arm axis, radius from the law of cosines. + float along = (upperLen * upperLen - foreLen * foreLen + d * d) / (2f * d); + float radius = Mathf.Sqrt(Mathf.Max(upperLen * upperLen - along * along, 0f)); + Vector3 center = shoulder + dir * along; + + Vector3 reference = Vector3.Down * 2f + torsoBasis * Vector3.Back * 0.8f + side * 0.3f; + reference -= dir * reference.Dot(dir); + if (reference.LengthSquared() > 0.06f) + _lastRef = reference.Normalized(); + else + { + // Arm pointing along the reference: keep the previous one, re-projected. + reference = _lastRef - dir * _lastRef.Dot(dir); + reference = reference.LengthSquared() > 1e-8f ? reference.Normalized() : dir.Cross(torsoBasis.X).Normalized(); + } + Vector3 refN = reference.Normalized(); + Vector3 refB = dir.Cross(refN); + + float targetAngle = 0f; + if (handBasis is Basis hb) + { + Vector3 handForward = (hb * Vector3.Forward).Normalized(); + Vector3 handElbow = wrist - handForward * foreLen - center; + handElbow -= dir * handElbow.Dot(dir); + float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f); + if (handElbow.LengthSquared() > 1e-8f) + { + // Read the suggestion as the representation NEAREST the current elbow angle: + // pointing the controller down puts it at the ±180° seam, where jitter would + // otherwise flip the clamped elbow to the other side. + float raw = Mathf.Atan2(handElbow.Dot(refB), handElbow.Dot(refN)); + raw = _poleAngle + Mathf.Wrap(raw - _poleAngle, -Mathf.Pi, Mathf.Pi); + targetAngle = raw * PoleHandWeight * confidence; + } + } + targetAngle = Mathf.Clamp(targetAngle, -PoleMaxAngle, PoleMaxAngle); + float step = (targetAngle - _poleAngle) * (1f - Mathf.Exp(-delta * PoleSmoothRate)); + _poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta); + Vector3 pole = refN.Rotated(dir, _poleAngle); + + Elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius; + Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen; + } + + // A real wrist flexes, so following the hand axis literally can throw the elbow above + // the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down + // its circle, preserving its sideways heading. + private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax) + { + if (center.Y + pole.Y * radius <= yMax) + return pole; + Vector3 vertical = Vector3.Up - dir * dir.Y; + if (vertical.LengthSquared() < 1e-6f) + return pole; // arm vertical: the circle is horizontal, height can't change + vertical = vertical.Normalized(); + Vector3 lateral = dir.Cross(vertical); + float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f); + float sin = Mathf.Sqrt(1f - cosMax * cosMax); + float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f; + return (vertical * cosMax + lateral * (sin * sideSign)).Normalized(); + } +} diff --git a/scripts/ArmSpike.cs b/scripts/ArmSpike.cs index b36a62c..7073688 100644 --- a/scripts/ArmSpike.cs +++ b/scripts/ArmSpike.cs @@ -12,7 +12,9 @@ public partial class ArmSpike : Node3D private const float StretchLeanRangeLimit = 0.01f; private const float HugElbowOutMin = 0.05f; private const float HoldPenetrationLimit = 0.01f; - private const float FlipElbowStepLimit = 0.03f; + // Unclamped pole flips jump 0.138 m/tick; the clamped, rate-limited pole stays well under this + // (0.029 with the old 0.72 m heuristic arms, 0.039 with profile-correct 0.57 m arms). + private const float FlipElbowStepLimit = 0.05f; private const float PinHandLagMin = 0.1f; private const float RestBodyPenLimit = 0.01f; private const float HoldAnvilY = 1.25f; // anvil top ≈ shoulder height, like a seated player at the table @@ -22,7 +24,7 @@ public partial class ArmSpike : Node3D private const float ChestDisturbLimit = 0.01f; private const float PushMinDisplacement = 0.05f; private const float PushHandClearance = 0.08f; - private const float PinSpeedLimit = 0.3f; + private const float PinSpeedLimit = 0.4f; private const float PinBarY = 1.22f; private const float PinObstructionMin = 0.01f; private const float TeleportGapLimit = 0.04f; diff --git a/scripts/BodyProfile.cs b/scripts/BodyProfile.cs new file mode 100644 index 0000000..b3199bd --- /dev/null +++ b/scripts/BodyProfile.cs @@ -0,0 +1,132 @@ +using System; +using System.IO; +using System.Text.Json; +using Godot; + +// The player's measured body (True Body metrics), from the one-time T-pose calibration. +// All heights are above the tracking floor; lengths in metres. Ratios: Winter, Biomechanics and +// Motor Control of Human Movement, Fig 4.1 (Drillis & Contini) — see research/nimso-project-tx.md §3. +public sealed class BodyProfile +{ + public const float EyeToStature = 0.936f; + public const float ShoulderWidthRatio = 0.259f; // biacromial, joint to joint + public const float ShoulderHeightRatio = 0.818f; + public const float UpperArmRatio = 0.186f; + public const float ForearmRatio = 0.146f; + public const float ArmSafetyMargin = 0.015f; // store arms slightly short: the real elbow must never lock first + + public int Version { get; set; } = 1; + public float EyeHeight { get; set; } + public float Stature { get; set; } + public float ShoulderHalfWidth { get; set; } + public float ShoulderDrop { get; set; } // eye height − shoulder-joint height + public float UpperArm { get; set; } + public float Forearm { get; set; } + public float MeasuredSpan { get; set; } // wrist-to-wrist in the T-pose (0 = estimated, not measured) + public string Date { get; set; } = ""; + + public float ArmLength => UpperArm + Forearm; + + // Current profile for the session; null until calibrated (main scene) or estimated (headless spikes). + public static BodyProfile Current { get; set; } + + public static event Action Changed; + + public static void Set(BodyProfile profile) + { + Current = profile; + Changed?.Invoke(profile); + } + + // Anthropometric estimate from eye height alone — used by headless spikes and as a fallback. + public static BodyProfile Estimate(float eyeHeight) + { + float stature = eyeHeight / EyeToStature; + float arm = (UpperArmRatio + ForearmRatio) * stature - ArmSafetyMargin; + return FromMeasurements(eyeHeight, stature, ShoulderWidthRatio * stature * 0.5f, + eyeHeight - ShoulderHeightRatio * stature, arm, 0f); + } + + // From a T-pose: wrist span = shoulder width + 2 arms. Shoulder width comes from the stature + // ratio — a few cm of error there only moves where the arm starts, not the total reach. + public static BodyProfile FromTPose(float eyeHeight, float wristSpan, float wristHeight) + { + float stature = eyeHeight / EyeToStature; + float halfWidth = ShoulderWidthRatio * stature * 0.5f; + float arm = (wristSpan - 2f * halfWidth) * 0.5f - ArmSafetyMargin; + return FromMeasurements(eyeHeight, stature, halfWidth, eyeHeight - wristHeight, arm, wristSpan); + } + + private static BodyProfile FromMeasurements(float eye, float stature, float halfWidth, float drop, float arm, float span) + { + float split = UpperArmRatio / (UpperArmRatio + ForearmRatio); + return new BodyProfile + { + EyeHeight = eye, + Stature = stature, + ShoulderHalfWidth = halfWidth, + ShoulderDrop = drop, + UpperArm = arm * split, + Forearm = arm * (1f - split), + MeasuredSpan = span, + Date = DateTime.Now.ToString("yyyy-MM-dd HH:mm"), + }; + } + + public bool IsPlausible() => + EyeHeight is > 1.0f and < 2.3f && ArmLength is > 0.35f and < 0.9f + && ShoulderHalfWidth is > 0.12f and < 0.32f && ShoulderDrop is > 0.05f and < 0.5f; + + public override string ToString() => + $"eye {EyeHeight:F3} m, stature {Stature:F2} m, arm {ArmLength:F3} m (upper {UpperArm:F3} / fore {Forearm:F3}), " + + $"shoulder half-width {ShoulderHalfWidth:F3} m, shoulder drop {ShoulderDrop:F3} m"; +} + +// prefs.json next to the executable (exported builds) or in the project folder (editor builds, +// incl. headless runs) — portable: copy the file to carry your calibration. Gitignored. +public static class Prefs +{ + private sealed class Document + { + public BodyProfile Body { get; set; } + } + + private static readonly JsonSerializerOptions Options = new() { WriteIndented = true }; + + public static string FilePath => + OS.HasFeature("editor") + ? ProjectSettings.GlobalizePath("res://prefs.json") + : Path.Combine(Path.GetDirectoryName(OS.GetExecutablePath()) ?? ".", "prefs.json"); + + public static BodyProfile LoadBody(string path = null) + { + path ??= FilePath; + try + { + if (!File.Exists(path)) + return null; + Document doc = JsonSerializer.Deserialize(File.ReadAllText(path), Options); + return doc?.Body != null && doc.Body.IsPlausible() ? doc.Body : null; + } + catch (Exception e) + { + GD.PushWarning($"Prefs: could not read {path}: {e.Message}"); + return null; + } + } + + public static bool SaveBody(BodyProfile body, string path = null) + { + path ??= FilePath; + try + { + File.WriteAllText(path, JsonSerializer.Serialize(new Document { Body = body }, Options)); + return true; + } + catch (Exception e) + { + GD.PushWarning($"Prefs: could not write {path}: {e.Message}"); + return false; + } + } +} diff --git a/scripts/BodyRig.cs b/scripts/BodyRig.cs index c24d770..20266e6 100644 --- a/scripts/BodyRig.cs +++ b/scripts/BodyRig.cs @@ -18,7 +18,8 @@ public partial class BodyRig : Node3D private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2]; private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2]; - private bool _sized; + private float _sizedUpper = -1f; + private float _sizedFore = -1f; private Basis _yawBasis = Basis.Identity; public override void _Ready() @@ -59,9 +60,10 @@ public partial class BodyRig : Node3D UpdateYawBasis(); if (!_arms.Calibrated || _arms.UpperArm(0) == null) return; - if (!_sized) + if (_sizedUpper != _arms.UpperLen || _sizedFore != _arms.ForeLen) { - _sized = true; + _sizedUpper = _arms.UpperLen; + _sizedFore = _arms.ForeLen; for (int side = 0; side < 2; side++) { ((BoxMesh)_upperArmMeshes[side].Mesh).Size = new Vector3(0.07f, _arms.UpperLen, 0.07f); diff --git a/scripts/Calibration.cs b/scripts/Calibration.cs new file mode 100644 index 0000000..5222c49 --- /dev/null +++ b/scripts/Calibration.cs @@ -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("../XROrigin3D/XRCamera3D"); + _left = GetNode("../XROrigin3D/LeftHand"); + _right = GetNode("../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")}"); + } +} diff --git a/scripts/CollapseSpike.cs b/scripts/CollapseSpike.cs index a102d33..6e176cc 100644 --- a/scripts/CollapseSpike.cs +++ b/scripts/CollapseSpike.cs @@ -58,6 +58,9 @@ public partial class CollapseSpike : Node3D public override void _Ready() { + // A real profile is measured standing once (T-pose) and does not shrink when seated: + // every scenario, seated or not, uses the same standing 1.65 m-eye profile. + BodyProfile.Current = BodyProfile.Estimate(1.65f); // Seated head 1.01 m (the playtest calibration); chest center z = head z + 0.12. // Table front edge is z = -0.2, chest half-depth 0.065: overlap starts at head z < -0.255. Add("standing, no table overlap, anvil", new(0.2f, 1.65f, 0.3f), 6f, anvil: true, table: true); diff --git a/scripts/PhysicsArms.cs b/scripts/PhysicsArms.cs index 063bfb8..5af205c 100644 --- a/scripts/PhysicsArms.cs +++ b/scripts/PhysicsArms.cs @@ -19,20 +19,14 @@ public partial class PhysicsArms : Node private const float SegmentInset = 0.04f; public const float SnapDistance = 1f; - private const float ShoulderRise = 0.25f; + public const float ShoulderRise = 0.25f; private const float ReachBudgetMax = 0.25f; private const float ReachVerticalMin = -0.05f; private const float ReachVerticalMax = 0.12f; private const float LeanReachFactor = 0.95f; private const float LeanSmoothRate = 6f; private const float WristOffset = 0.06f; - private const float SoftIKFactor = 0.98f; - private const float PoleHandWeight = 0.85f; - private const float PoleSmoothRate = 12f; - private const float PoleMaxAngle = 1.92f; // 110 deg either way from down/back/out - private const float PoleMaxSpeed = 6f; // rad/s public const float HandArmSlack = 0.03f; - private const float ElbowRiseAllowance = 0.05f; public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg; public static float UpperArmMaxForce => BodyMassStat * UpperArmForcePerKg; @@ -57,8 +51,7 @@ public partial class PhysicsArms : Node private readonly Vector3[] _shoulder = new Vector3[2]; private readonly Vector3[] _elbowTarget = new Vector3[2]; private readonly Vector3[] _wristTarget = new Vector3[2]; - private readonly float[] _poleAngle = new float[2]; - private readonly Vector3[] _lastRef = new Vector3[2]; + private readonly ArmSolver[] _solver = { new(), new() }; private readonly RigidBody3D[] _held = new RigidBody3D[2]; private Vector3 _torsoOffset; private Basis _torsoBasis = Basis.Identity; @@ -75,8 +68,12 @@ public partial class PhysicsArms : Node public Vector3 Wrist(int side) => SegmentEnd(_fore[side], ForeLen, 1f); + private bool _injected; + public override void _Ready() { + _injected = HeadSource != null; + BodyProfile.Changed += OnProfileChanged; if (HeadSource == null) { HeadSource = GetNode("../XROrigin3D/XRCamera3D"); @@ -93,10 +90,12 @@ public partial class PhysicsArms : Node float dt = (float)delta; if (!Calibrated) { - float headHeight = HeadSource.GlobalPosition.Y; - if (headHeight <= 0.5f || Torso?.Body == null) + // Main scene: wait for the T-pose profile. Headless spikes (HeadSource injected) use + // the anthropometric estimate from their head height. + BodyProfile profile = BodyProfile.Current ?? (_injected ? BodyProfile.Estimate(HeadSource.GlobalPosition.Y) : null); + if (profile == null || Torso?.Body == null) return; - Calibrate(headHeight); + Calibrate(profile); } _torsoBasis = Torso.Body.GlobalTransform.Basis; @@ -115,13 +114,31 @@ public partial class PhysicsArms : Node } } - private void Calibrate(float headHeight) + public override void _ExitTree() + { + BodyProfile.Changed -= OnProfileChanged; + } + + // Recalibration: rebuild the segments at the new lengths on the next tick. + private void OnProfileChanged(BodyProfile profile) + { + if (!Calibrated) + return; + Calibrated = false; + for (int side = 0; side < 2; side++) + { + _upper[side]?.QueueFree(); + _fore[side]?.QueueFree(); + _upper[side] = _fore[side] = null; + } + } + + private void Calibrate(BodyProfile profile) { Calibrated = true; - float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f); - UpperLen = 0.5f * armTotal; - ForeLen = 0.5f * armTotal; - ShoulderHalfWidth = 0.16f + headHeight * 0.02f; + UpperLen = profile.UpperArm; + ForeLen = profile.Forearm; + ShoulderHalfWidth = profile.ShoulderHalfWidth; for (int side = 0; side < 2; side++) { @@ -130,9 +147,7 @@ public partial class PhysicsArms : Node _fore[side] = MakeSegment($"{name}ForearmBody", ForearmMass, new Vector3(0.06f, ForeLen - SegmentInset, 0.06f)); SetHeldExceptions(side, _held[side], true); } - GD.Print($"PhysicsArms: proportions calibrated from head height {headHeight:F2} m " + - $"(arm total {armTotal:F2} m, upper {UpperLen:F2} m, fore {ForeLen:F2} m, shoulder half-width {ShoulderHalfWidth:F2} m); " + - $"budgets fore {ForearmMaxForce:F0} N, upper {UpperArmMaxForce:F0} N"); + GD.Print($"PhysicsArms: body profile {profile}; budgets fore {ForearmMaxForce:F0} N, upper {UpperArmMaxForce:F0} N"); } private RigidBody3D MakeSegment(string name, float mass, Vector3 size) @@ -290,81 +305,9 @@ public partial class PhysicsArms : Node { Vector3 shoulder = _shoulder[index]; Vector3 rawTarget = hand != null ? WristPoint(hand) : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); - - float reach = UpperLen + ForeLen; - Vector3 shoulderToTarget = rawTarget - shoulder; - if (shoulderToTarget.LengthSquared() < 0.0001f) - shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f); - - float rawDist = shoulderToTarget.Length(); - float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor); - Vector3 dir = shoulderToTarget / rawDist; - Vector3 wrist = shoulder + dir * d; - - // Elbow circle: center on the arm axis, radius from the law of cosines. - float along = (UpperLen * UpperLen - ForeLen * ForeLen + d * d) / (2f * d); - float radius = Mathf.Sqrt(Mathf.Max(UpperLen * UpperLen - along * along, 0f)); - Vector3 center = shoulder + dir * along; - - // Reference = torso-frame down/back/out projected off the arm axis. The elbow is - // stored as an ANGLE around the arm axis relative to it, clamped to ±110° and - // rate-limited — it can never swing through the inward/up side (the 180° flip). - Vector3 reference = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f; - reference -= dir * reference.Dot(dir); - if (reference.LengthSquared() > 0.06f) - _lastRef[index] = reference.Normalized(); - else - { - // Arm pointing along the reference: keep the previous one, re-projected. - reference = _lastRef[index] - dir * _lastRef[index].Dot(dir); - reference = reference.LengthSquared() > 1e-8f ? reference.Normalized() : dir.Cross(_torsoBasis.X).Normalized(); - } - Vector3 refN = reference.Normalized(); - Vector3 refB = dir.Cross(refN); - - float targetAngle = 0f; - Basis handBasis = orient != null ? orient.GlobalTransform.Basis : hand?.GlobalTransform.Basis ?? Basis.Identity; - if (orient != null || hand != null) - { - Vector3 handForward = (handBasis * Vector3.Forward).Normalized(); - Vector3 handElbow = wrist - handForward * ForeLen - center; - handElbow -= dir * handElbow.Dot(dir); - float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f); - if (handElbow.LengthSquared() > 1e-8f) - { - // Read the suggestion as the representation NEAREST the current elbow angle: - // pointing the controller down puts it at the ±180° seam, where jitter would - // otherwise flip the clamped elbow to the other side. - float raw = Mathf.Atan2(handElbow.Dot(refB), handElbow.Dot(refN)); - raw = _poleAngle[index] + Mathf.Wrap(raw - _poleAngle[index], -Mathf.Pi, Mathf.Pi); - targetAngle = raw * PoleHandWeight * confidence; - } - } - targetAngle = Mathf.Clamp(targetAngle, -PoleMaxAngle, PoleMaxAngle); - float step = (targetAngle - _poleAngle[index]) * (1f - Mathf.Exp(-delta * PoleSmoothRate)); - _poleAngle[index] += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta); - Vector3 pole = refN.Rotated(dir, _poleAngle[index]); - - Vector3 elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius; - _elbowTarget[index] = elbow; - _wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen; - } - - // A real wrist flexes, so following the hand axis literally can throw the elbow above - // the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down - // its circle, preserving its sideways heading. - private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax) - { - if (center.Y + pole.Y * radius <= yMax) - return pole; - Vector3 vertical = Vector3.Up - dir * dir.Y; - if (vertical.LengthSquared() < 1e-6f) - return pole; // arm vertical: the circle is horizontal, height can't change - vertical = vertical.Normalized(); - Vector3 lateral = dir.Cross(vertical); - float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f); - float sin = Mathf.Sqrt(1f - cosMax * cosMax); - float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f; - return (vertical * cosMax + lateral * (sin * sideSign)).Normalized(); + Basis? handBasis = orient != null ? orient.GlobalTransform.Basis : hand?.GlobalTransform.Basis; + _solver[index].Solve(shoulder, rawTarget, _torsoBasis, side, handBasis, UpperLen, ForeLen, delta); + _elbowTarget[index] = _solver[index].Elbow; + _wristTarget[index] = _solver[index].Wrist; } } diff --git a/scripts/PhysicsTorso.cs b/scripts/PhysicsTorso.cs index 245f9d3..5d9774b 100644 --- a/scripts/PhysicsTorso.cs +++ b/scripts/PhysicsTorso.cs @@ -6,7 +6,6 @@ public partial class PhysicsTorso : Node private const float MaxForce = 2400f; private const float MaxSpeed = 6f; public const float SnapDistance = 2f; - private const float TorsoDrop = 0.42f; private const float TorsoBackOffset = 0.06f; private const float HeadBackOffset = 0.06f; public const float YawHandsWeight = 0.6f; @@ -82,12 +81,20 @@ public partial class PhysicsTorso : Node Node3D head = HeadSource != null ? HeadSource : (Node3D)_camera; if (head == null || _body == null) return; + // Main scene: wait for the calibrated profile (the T-pose gate). Headless spikes inject + // HeadSource and get the anthropometric estimate from their head height instead. + BodyProfile profile = BodyProfile.Current ?? (HeadSource != null ? BodyProfile.Estimate(head.GlobalPosition.Y) : null); + if (profile == null) + return; + // Chest centre sits so that its shoulder line (centre + ShoulderRise) is at the + // measured shoulder height below the eyes. + float torsoDrop = profile.ShoulderDrop + PhysicsArms.ShoulderRise; float dt = (float)delta; _yawBasis = FlatYawBasis(head.GlobalTransform.Basis); Vector3 headPos = head.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset; UpdateTorsoYaw(headPos, dt); - NominalTarget = headPos + _torsoBasis * new Vector3(0f, -TorsoDrop, TorsoBackOffset); + NominalTarget = headPos + _torsoBasis * new Vector3(0f, -torsoDrop, TorsoBackOffset); Vector3 target = NominalTarget + ReachOffset; TargetPosition = target; diff --git a/scripts/TrueBody.cs b/scripts/TrueBody.cs new file mode 100644 index 0000000..d8102bf --- /dev/null +++ b/scripts/TrueBody.cs @@ -0,0 +1,247 @@ +using Godot; + +// The derived pose of the player's REAL body (Nimso's "True Body"): from the head + 2 controllers + +// the calibrated BodyProfile, trigonometry — not full IK — places neck, chest (yaw + crouch lean), +// pelvis (lagging spring → sway), a MOVING shoulder girdle and the arms. Pure solver; the physics +// ragdoll (step 2) drives its joints toward this pose. All positions are world space. +public sealed class TrueBodySolver +{ + // Head geometry (head-local, from the eyes): the neck pivot sits behind and below the eyes. + public static readonly Vector3 EyeToNeckPivot = new(0f, -0.09f, 0.07f); + public const float NeckBaseAboveShoulders = 0.04f; + public const float ShouldersBehindEyes = 0.06f; + public const float TrunkRatio = 0.288f; // shoulder joint − hip joint height, × stature (Winter 0.818 − 0.530) + public const float ChestLeanMax = 0.75f; // rad (~43°) at a deep crouch + public const float CrouchRange = 0.5f; // crouch factor 1 when eyes are this fraction of standing eye height lower + public const float HeadPitchToChest = 0.25f; + public const float PelvisFrequency = 2.5f; // Hz, critically damped follow → hip lag/sway + public const float GirdleReachStart = 0.98f; // fraction of arm length where the shoulder starts to help + public const float GirdleSmoothRate = 15f; + // Girdle travel limits in the chest frame (outward, up, forward), metres. [I] anatomy-scale guesses. + public const float GirdleOut = 0.03f, GirdleIn = 0.02f, GirdleUp = 0.07f, GirdleDown = 0.02f, GirdleForward = 0.08f, GirdleBack = 0.03f; + public const float RhythmLift = 0.04f; // extra shrug with the arm fully overhead (scapulohumeral rhythm) + public const float YawHandsWeight = 0.6f; + public const float YawForwardness = 0.3f; + public const float YawSmoothRate = 12f; + public const float WristOffset = 0.06f; + + public Vector3 Eye, NeckPivot, NeckBase, UpperChest, Pelvis; + public Basis ChestBasis = Basis.Identity; + public Basis PelvisBasis = Basis.Identity; + public readonly Vector3[] Shoulder = new Vector3[2]; + public readonly Vector3[] Elbow = new Vector3[2]; + public readonly Vector3[] Wrist = new Vector3[2]; + public readonly Vector3[] WristTarget = new Vector3[2]; + public float Crouch { get; private set; } + public bool Initialized { get; private set; } + + private readonly ArmSolver[] _arms = { new(), new() }; + private readonly Vector3[] _girdle = new Vector3[2]; // chest-local shoulder offset + private Vector3 _pelvisVelocity; + private float _yaw; + + public void Reset() + { + Initialized = false; + _girdle[0] = _girdle[1] = Vector3.Zero; + _pelvisVelocity = Vector3.Zero; + } + + // floorY = world height of the tracking floor (XROrigin3D). Controllers are aim poses. + public void Solve(BodyProfile p, Transform3D head, Transform3D left, Transform3D right, float floorY, float dt) + { + Eye = head.Origin; + Basis headBasis = head.Basis.Orthonormalized(); + NeckPivot = Eye + headBasis * EyeToNeckPivot; + WristTarget[0] = left * new Vector3(0f, 0f, WristOffset); + WristTarget[1] = right * new Vector3(0f, 0f, WristOffset); + + // Torso yaw: HMD yaw blended toward the hands when they're out in front (decoupled from the head). + Vector3 headFwd = -headBasis.Z; + float hmdYaw = Mathf.Atan2(-headFwd.X, -headFwd.Z); + float targetYaw = hmdYaw; + Vector3 toHands = (WristTarget[0] + WristTarget[1]) * 0.5f - Eye; + toHands.Y = 0f; + Vector3 flatFwd = new Vector3(headFwd.X, 0f, headFwd.Z); + if (toHands.LengthSquared() > 1e-4f && flatFwd.LengthSquared() > 1e-6f) + { + float handsYaw = Mathf.Atan2(-toHands.X, -toHands.Z); + float forwardness = Mathf.Clamp(toHands.Dot(flatFwd.Normalized()) / YawForwardness, 0f, 1f); + targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight); + } + if (!Initialized) + _yaw = hmdYaw; + _yaw = Mathf.LerpAngle(_yaw, targetYaw, 1f - Mathf.Exp(-dt * YawSmoothRate)); + Basis yawBasis = new Basis(Vector3.Up, _yaw); + + // Crouch lean: measured at the NECK PIVOT (nodding moves the eyes ~10 cm but not the pivot), + // below its standing height the chest pitches forward; looking down adds a little. + float pivotAboveFloor = NeckPivot.Y - floorY; + float standingPivot = p.EyeHeight + EyeToNeckPivot.Y; + Crouch = Mathf.Clamp((standingPivot - pivotAboveFloor) / (CrouchRange * p.EyeHeight), 0f, 1f); + float headPitchDown = Mathf.Max(0f, -Mathf.Asin(Mathf.Clamp(headFwd.Y, -1f, 1f))); + float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest; + ChestBasis = yawBasis * new Basis(Vector3.Right, -lean); + + // Neck: from the head pivot down along the average of chest-up and head-up, sized so a + // standing, level head puts the shoulder joints at the measured drop below the eyes. + float neckLen = Mathf.Max(0.05f, p.ShoulderDrop + EyeToNeckPivot.Y - NeckBaseAboveShoulders); + Vector3 neckDir = (ChestBasis.Y + headBasis.Y).Normalized(); + NeckBase = NeckPivot - neckDir * neckLen; + // Keep the shoulder line behind the eyes by the anthropometric offset (neck pivot already + // supplies part of it). + UpperChest = NeckBase - ChestBasis.Y * NeckBaseAboveShoulders + + ChestBasis * new Vector3(0f, 0f, ShouldersBehindEyes - EyeToNeckPivot.Z); + + // Pelvis: hangs below the upper chest along the trunk; follows through a critically damped + // spring so it lags quick head motion (hip sway) instead of being rigidly attached. + Vector3 pelvisTarget = UpperChest - ChestBasis.Y * (TrunkRatio * p.Stature); + if (!Initialized) + Pelvis = pelvisTarget; + float w = Mathf.Tau * PelvisFrequency; + Vector3 accel = (pelvisTarget - Pelvis) * (w * w) - _pelvisVelocity * (2f * w); + _pelvisVelocity += accel * dt; + Pelvis += _pelvisVelocity * dt; + PelvisBasis = yawBasis; + + for (int side = 0; side < 2; side++) + SolveArm(p, side, side == 0 ? left.Basis : right.Basis, dt); + Initialized = true; + } + + private void SolveArm(BodyProfile p, int side, Basis handBasis, float dt) + { + float sign = side == 0 ? -1f : 1f; + Vector3 outward = ChestBasis.X * sign; + Vector3 neutral = UpperChest + outward * p.ShoulderHalfWidth; + + // Girdle: once the hand is beyond ~full reach the shoulder joint shifts toward it, within + // anatomical travel, plus a small shrug as the arm goes overhead. + Vector3 toWrist = WristTarget[side] - neutral; + float excess = toWrist.Length() - p.ArmLength * GirdleReachStart; + Vector3 desired = Vector3.Zero; + if (excess > 0f && toWrist.LengthSquared() > 1e-8f) + { + Vector3 local = ChestBasis.Transposed() * (toWrist.Normalized() * excess); + float outLocal = Mathf.Clamp(local.X * sign, -GirdleIn, GirdleOut); + desired = new Vector3(outLocal * sign, + Mathf.Clamp(local.Y, -GirdleDown, GirdleUp), + Mathf.Clamp(local.Z, -GirdleForward, GirdleBack)); + } + if (toWrist.LengthSquared() > 1e-8f) + { + float raise = Mathf.Clamp((toWrist.Normalized().Dot(ChestBasis.Y) + 0.5f) / 1.5f, 0f, 1f); // 0 at ~30° below horizontal, 1 overhead + desired.Y = Mathf.Min(desired.Y + raise * RhythmLift, GirdleUp); + } + _girdle[side] = Initialized ? _girdle[side].Lerp(desired, 1f - Mathf.Exp(-dt * GirdleSmoothRate)) : desired; + Shoulder[side] = neutral + ChestBasis * _girdle[side]; + + _arms[side].Solve(Shoulder[side], WristTarget[side], ChestBasis, outward, handBasis, p.UpperArm, p.Forearm, dt); + Elbow[side] = _arms[side].Elbow; + Wrist[side] = _arms[side].Wrist; + } +} + +// Runs the TrueBodySolver every physics tick once a profile exists and draws a translucent debug +// skeleton of it (ShowDebug) so the derived pose can be judged in-headset. +public partial class TrueBody : Node3D +{ + public static bool ShowDebug = true; + + private XROrigin3D _origin; + private XRCamera3D _camera; + private XRController3D _left; + private XRController3D _right; + private StandardMaterial3D _material; + private readonly System.Collections.Generic.List _bones = new(); + private readonly System.Collections.Generic.List _joints = new(); + + public TrueBodySolver Solver { get; } = new(); + public bool HasPose => Solver.Initialized; + + public override void _Ready() + { + _origin = GetNode("../XROrigin3D"); + _camera = GetNode("../XROrigin3D/XRCamera3D"); + _left = GetNode("../XROrigin3D/LeftHand"); + _right = GetNode("../XROrigin3D/RightHand"); + _material = new StandardMaterial3D + { + AlbedoColor = new Color(0.3f, 0.9f, 1f, 0.35f), + Transparency = BaseMaterial3D.TransparencyEnum.Alpha, + ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded, + NoDepthTest = false, + }; + BodyProfile.Changed += _ => Solver.Reset(); + } + + public override void _PhysicsProcess(double delta) + { + BodyProfile profile = BodyProfile.Current; + if (profile == null) + return; + Solver.Solve(profile, _camera.GlobalTransform, _left.GlobalTransform, _right.GlobalTransform, _origin.GlobalPosition.Y, (float)delta); + } + + public override void _Process(double delta) + { + bool show = ShowDebug && HasPose; + if (!show) + { + foreach (MeshInstance3D m in _bones) + m.Visible = false; + foreach (MeshInstance3D m in _joints) + m.Visible = false; + return; + } + TrueBodySolver s = Solver; + int b = 0, j = 0; + Bone(ref b, s.NeckPivot, s.NeckBase, 0.025f); + Bone(ref b, s.NeckBase, s.UpperChest, 0.03f); + Bone(ref b, s.UpperChest, s.Pelvis, 0.05f); + for (int side = 0; side < 2; side++) + { + Bone(ref b, s.UpperChest, s.Shoulder[side], 0.025f); + Bone(ref b, s.Shoulder[side], s.Elbow[side], 0.03f); + Bone(ref b, s.Elbow[side], s.Wrist[side], 0.025f); + Joint(ref j, s.Shoulder[side]); + Joint(ref j, s.Elbow[side]); + Joint(ref j, s.Wrist[side]); + } + Joint(ref j, s.Pelvis, 0.06f); + Joint(ref j, s.NeckPivot); + } + + private void Bone(ref int index, Vector3 from, Vector3 to, float thickness) + { + if (index >= _bones.Count) + { + MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = Vector3.One }, MaterialOverride = _material, CastShadow = GeometryInstance3D.ShadowCastingSetting.Off }; + AddChild(mesh); + _bones.Add(mesh); + } + MeshInstance3D bone = _bones[index++]; + Vector3 seg = to - from; + float len = seg.Length(); + bone.Visible = len > 1e-4f; + if (!bone.Visible) + return; + Vector3 y = seg / len; + Vector3 x = Mathf.Abs(y.Y) < 0.99f ? Vector3.Up.Cross(y).Normalized() : Vector3.Right.Cross(y).Normalized(); + Vector3 z = x.Cross(y); + bone.GlobalTransform = new Transform3D(new Basis(x * thickness, y * len, z * thickness), (from + to) * 0.5f); + } + + private void Joint(ref int index, Vector3 at, float size = 0.04f) + { + if (index >= _joints.Count) + { + MeshInstance3D mesh = new() { Mesh = new SphereMesh { Radius = 0.5f, Height = 1f }, MaterialOverride = _material, CastShadow = GeometryInstance3D.ShadowCastingSetting.Off }; + AddChild(mesh); + _joints.Add(mesh); + } + MeshInstance3D joint = _joints[index++]; + joint.Visible = true; + joint.GlobalTransform = new Transform3D(Basis.Identity.Scaled(Vector3.One * size), at); + } +} diff --git a/scripts/TrueBodySpike.cs b/scripts/TrueBodySpike.cs new file mode 100644 index 0000000..baa4878 --- /dev/null +++ b/scripts/TrueBodySpike.cs @@ -0,0 +1,204 @@ +using System.Collections.Generic; +using System.IO; +using Godot; + +// Headless regression for body step 1: T-pose calibration (TPoseDetector), prefs.json persistence, +// and the TrueBodySolver pose. No physics — runs synchronously and quits. +// godot-mono --headless --path . scenes/truebody_spike.tscn +public partial class TrueBodySpike : Node +{ + private const float Dt = 1f / 90f; + private const float Stature = 1.80f; + private static float Eye => BodyProfile.EyeToStature * Stature; + private static float ShoulderY => BodyProfile.ShoulderHeightRatio * Stature; + private static float HalfWidth => BodyProfile.ShoulderWidthRatio * Stature * 0.5f; + private static float RealArm => (BodyProfile.UpperArmRatio + BodyProfile.ForearmRatio) * Stature; + + private readonly List<(string Name, bool Ok)> _results = new(); + private readonly RandomNumberGenerator _rng = new() { Seed = 7 }; + + public override void _Ready() + { + BodyProfile measured = TestDetector(); + TestPersistence(measured); + TestSolver(measured ?? BodyProfile.Estimate(Eye)); + bool all = true; + foreach (var (name, ok) in _results) + { + GD.Print($"TRUEBODY: {name} → {(ok ? "PASS" : "FAIL")}"); + all &= ok; + } + GD.Print($"TRUEBODY: overall {(all ? "PASS — calibration + True Body functional" : "CHECK — inspect failures above")}"); + GetTree().Quit(); + } + + private void Record(string name, bool ok) => _results.Add((name, ok)); + + private Vector3 Noise(float amp) => new(_rng.RandfRange(-amp, amp), _rng.RandfRange(-amp, amp), _rng.RandfRange(-amp, amp)); + + // ---- calibration ---- + private BodyProfile RunPose(float seconds, System.Func pose, TPoseDetector d) + { + for (float t = 0f; t < seconds; t += Dt) + { + var (head, fwd, l, r) = pose(t); + BodyProfile p = d.Feed(head, fwd, l, r, Dt); + if (p != null) + return p; + } + return null; + } + + private (Vector3, Vector3, Vector3, Vector3) TPose(float t, float leftDrop = 0f, float wobble = 0f, bool armsForward = false, float spanScale = 1f) + { + float reach = (HalfWidth + RealArm) * spanScale; + Vector3 head = new Vector3(0f, Eye, 0f) + Noise(0.0005f); + Vector3 l = armsForward ? new Vector3(-HalfWidth, ShoulderY, -RealArm) : new Vector3(-reach, ShoulderY - leftDrop, 0f); + Vector3 r = armsForward ? new Vector3(HalfWidth, ShoulderY, -RealArm) : new Vector3(reach, ShoulderY, 0f); + Vector3 w = new(0f, 0.05f * Mathf.Sin(t * Mathf.Tau * 2f) * wobble, 0f); + return (head, Vector3.Forward, l + w + Noise(0.0005f), r - w + Noise(0.0005f)); + } + + private BodyProfile TestDetector() + { + var d = new TPoseDetector(); + BodyProfile p = RunPose(1.6f, t => TPose(t), d); + float expectArm = RealArm - BodyProfile.ArmSafetyMargin; + bool ok = p != null && Mathf.Abs(p.EyeHeight - Eye) < 0.003f && Mathf.Abs(p.ArmLength - expectArm) < 0.005f + && Mathf.Abs(p.ShoulderDrop - (Eye - ShoulderY)) < 0.005f && Mathf.Abs(p.Stature - Stature) < 0.005f; + Record($"T-pose measured: {(p != null ? p.ToString() : "none")} (expected arm {expectArm:F3}, drop {Eye - ShoulderY:F3})", ok); + + d.Reset(); + Record($"uneven arms rejected ({(RunPose(3f, t => TPose(t, leftDrop: 0.15f), d) == null ? d.Problem : "accepted")})", + RunPose(0.01f, t => TPose(t, leftDrop: 0.15f), d) == null && d.Problem == "level your arms"); + d.Reset(); + Record($"moving arms rejected ({(RunPose(3f, t => TPose(t, wobble: 1f), d) == null ? d.Problem : "accepted")})", + RunPose(0.5f, t => TPose(t, wobble: 1f), d) == null); + d.Reset(); + Record("arms-forward pose rejected", RunPose(3f, t => TPose(t, armsForward: true), d) == null); + d.Reset(); + Record("implausible span rejected", RunPose(3f, t => TPose(t, spanScale: 1.6f), d) == null); + return p; + } + + private void TestPersistence(BodyProfile p) + { + if (p == null) + { + Record("persistence (no profile to save)", false); + return; + } + string path = Path.Combine(OS.GetUserDataDir(), "truebody_spike_prefs.json"); + bool saved = Prefs.SaveBody(p, path); + BodyProfile back = Prefs.LoadBody(path); + bool same = back != null && Mathf.IsEqualApprox(back.ArmLength, p.ArmLength) && Mathf.IsEqualApprox(back.EyeHeight, p.EyeHeight) + && Mathf.IsEqualApprox(back.ShoulderHalfWidth, p.ShoulderHalfWidth) && Mathf.IsEqualApprox(back.ShoulderDrop, p.ShoulderDrop); + Record($"prefs round-trip ({path})", saved && same); + File.WriteAllText(path, "{ not json"); + Record("corrupt prefs rejected", Prefs.LoadBody(path) == null); + File.Delete(path); + } + + // ---- solver ---- + private static Transform3D Ctrl(Vector3 wrist, Basis basis) => new(basis, wrist - basis * new Vector3(0f, 0f, TrueBodySolver.WristOffset)); + + private void Settle(TrueBodySolver s, BodyProfile p, Transform3D head, Transform3D l, Transform3D r, float seconds = 1.5f) + { + for (float t = 0f; t < seconds; t += Dt) + s.Solve(p, head, l, r, 0f, Dt); + } + + private void TestSolver(BodyProfile p) + { + var s = new TrueBodySolver(); + Transform3D head = new(Basis.Identity, new Vector3(0f, p.EyeHeight, 0f)); + Basis down = new(Vector3.Right, -Mathf.Pi / 2f); // controller pointing at the floor (hands hanging) + float shoulderY = p.EyeHeight - p.ShoulderDrop; + + // Neutral: hands hanging at the sides. + Vector3 hangL = new(-p.ShoulderHalfWidth, shoulderY - p.ArmLength * 0.95f, 0.06f); + Vector3 hangR = new(p.ShoulderHalfWidth, shoulderY - p.ArmLength * 0.95f, 0.06f); + Settle(s, p, head, Ctrl(hangL, down), Ctrl(hangR, down)); + Vector3 expectR = new(p.ShoulderHalfWidth, shoulderY, TrueBodySolver.ShouldersBehindEyes); + float shoulderErr = Mathf.Max(s.Shoulder[1].DistanceTo(expectR), s.Shoulder[0].DistanceTo(new Vector3(-expectR.X, expectR.Y, expectR.Z))); + float pelvisExpectY = shoulderY - TrueBodySolver.TrunkRatio * p.Stature; + Record($"neutral: shoulder joints within {shoulderErr * 100f:F1} cm of anthropometric (limit 2), pelvis y {s.Pelvis.Y:F3} vs {pelvisExpectY:F3}", + shoulderErr < 0.02f && Mathf.Abs(s.Pelvis.Y - pelvisExpectY) < 0.03f); + Vector3 neutralShoulderR = s.Shoulder[1]; + + // Reach straight forward 10 cm beyond the arm: the shoulder protracts, within its limit. + Basis fwd = Basis.Identity; + Vector3 farR = new(p.ShoulderHalfWidth, shoulderY, TrueBodySolver.ShouldersBehindEyes - p.ArmLength - 0.10f); + Settle(s, p, head, Ctrl(hangL, down), Ctrl(farR, fwd)); + float protraction = neutralShoulderR.Z - s.Shoulder[1].Z; + float wristErr = s.Wrist[1].DistanceTo(farR); + Record($"far reach: shoulder protracts {protraction * 100f:F1} cm (4–8.5), wrist short of target by {wristErr * 100f:F1} cm (< 5)", + protraction > 0.04f && protraction < TrueBodySolver.GirdleForward + 0.005f && wristErr < 0.05f); + + // Arm straight overhead: shrug. + Vector3 upR = new(p.ShoulderHalfWidth, shoulderY + p.ArmLength * 0.97f, TrueBodySolver.ShouldersBehindEyes); + Settle(s, p, head, Ctrl(hangL, down), Ctrl(upR, new Basis(Vector3.Right, Mathf.Pi / 2f))); + float shrug = s.Shoulder[1].Y - neutralShoulderR.Y; + Record($"overhead: shoulder rises {shrug * 100f:F1} cm (3–7.5)", shrug > 0.03f && shrug < TrueBodySolver.GirdleUp + 0.005f); + + // Look down 60° while standing: chest barely follows, shoulders stay put. + // A real head pivots at the neck: rotate about the neck pivot, the eyes travel. + Basis pitch = new(Vector3.Right, -Mathf.DegToRad(60f)); + Vector3 pivot = head.Origin + TrueBodySolver.EyeToNeckPivot; + Transform3D lookDown = new(pitch, pivot - pitch * TrueBodySolver.EyeToNeckPivot); + Settle(s, p, lookDown, Ctrl(hangL, down), Ctrl(hangR, down)); + float chestPitch = Mathf.RadToDeg(s.ChestBasis.Y.AngleTo(Vector3.Up)); + Record($"look down 60°: chest pitch {chestPitch:F1}° (< 20), shoulder height change {(s.Shoulder[1].Y - neutralShoulderR.Y) * 100f:F1} cm (|·| < 6)", + chestPitch < 20f && Mathf.Abs(s.Shoulder[1].Y - neutralShoulderR.Y) < 0.06f); + + // Deep crouch: eyes 0.6 m lower → chest leans, hips kick back behind the upper chest. + Transform3D crouch = new(Basis.Identity, new Vector3(0f, p.EyeHeight - 0.6f, 0f)); + Settle(s, p, crouch, Ctrl(hangL + Vector3.Down * 0.6f, down), Ctrl(hangR + Vector3.Down * 0.6f, down)); + chestPitch = Mathf.RadToDeg(s.ChestBasis.Y.AngleTo(Vector3.Up)); + float hipBack = s.Pelvis.Z - s.UpperChest.Z; + Record($"crouch: factor {s.Crouch:F2} (> 0.5), chest pitch {chestPitch:F1}° (> 20), hips {hipBack * 100f:F0} cm behind upper chest (> 10)", + s.Crouch > 0.5f && chestPitch > 20f && hipBack > 0.10f); + + // Pelvis lag: head snaps 0.3 m sideways → pelvis trails, then settles. + Settle(s, p, head, Ctrl(hangL, down), Ctrl(hangR, down)); + Transform3D side = new(Basis.Identity, head.Origin + new Vector3(0.3f, 0f, 0f)); + Vector3 before = s.Pelvis; + s.Solve(p, side, Ctrl(hangL + Vector3.Right * 0.3f, down), Ctrl(hangR + Vector3.Right * 0.3f, down), 0f, Dt); + float firstStep = s.Pelvis.DistanceTo(before); + Settle(s, p, side, Ctrl(hangL + Vector3.Right * 0.3f, down), Ctrl(hangR + Vector3.Right * 0.3f, down), 1f); + float settled = Mathf.Abs(s.Pelvis.X - (s.UpperChest.X)); + Record($"pelvis lag: first tick moves {firstStep * 100f:F1} cm of 30 (< 10), settled offset {settled * 100f:F1} cm (< 1)", firstStep < 0.10f && settled < 0.01f); + + // Random motion: finite, exact segment lengths, no elbow jumps. + bool finite = true; + float maxLenErr = 0f, maxElbowStep = 0f; + Vector3 lastElbow = s.Elbow[1]; + for (int i = 0; i < 900; i++) + { + float t = i * Dt; + Vector3 hp = new(0.2f * Mathf.Sin(t * 0.7f), p.EyeHeight - 0.3f * (0.5f + 0.5f * Mathf.Sin(t * 0.4f)), 0.15f * Mathf.Sin(t * 0.5f)); + Basis hb = new Basis(Vector3.Up, 0.8f * Mathf.Sin(t * 0.3f)) * new Basis(Vector3.Right, 0.4f * Mathf.Sin(t * 0.9f)); + // Hands roam the anatomically reachable space in front of the body (a wrist can't come + // within ~15 cm of its own shoulder joint; targets that do make the folded arm whip round). + Vector3 lw = hp + new Vector3(-0.3f + 0.2f * Mathf.Sin(t * 1.3f), -0.35f + 0.35f * Mathf.Sin(t * 1.1f), -0.4f + 0.15f * Mathf.Cos(t * 0.8f)); + Vector3 rw = hp + new Vector3(0.3f + 0.2f * Mathf.Cos(t * 1.7f), -0.35f + 0.35f * Mathf.Cos(t * 0.9f), -0.4f + 0.15f * Mathf.Sin(t * 1.2f)); + Basis lb = new Basis(new Vector3(0.3f, 1f, 0.2f).Normalized(), t * 2f); + Basis rb = new Basis(new Vector3(-0.2f, 0.4f, 1f).Normalized(), t * 1.5f); + s.Solve(p, new Transform3D(hb, hp), Ctrl(lw, lb), Ctrl(rw, rb), 0f, Dt); + for (int a = 0; a < 2; a++) + { + finite &= s.Shoulder[a].IsFinite() && s.Elbow[a].IsFinite() && s.Wrist[a].IsFinite() && s.Pelvis.IsFinite(); + maxLenErr = Mathf.Max(maxLenErr, Mathf.Abs(s.Shoulder[a].DistanceTo(s.Elbow[a]) - p.UpperArm)); + maxLenErr = Mathf.Max(maxLenErr, Mathf.Abs(s.Elbow[a].DistanceTo(s.Wrist[a]) - p.Forearm)); + } + if (i > 0) + { + float stepNow = s.Elbow[1].DistanceTo(lastElbow); + maxElbowStep = Mathf.Max(maxElbowStep, stepNow); + } + lastElbow = s.Elbow[1]; + } + Record($"random motion: finite {finite}, segment length error max {maxLenErr * 1000f:F2} mm (< 1), elbow step max {maxElbowStep * 100f:F1} cm/tick (< 6)", + finite && maxLenErr < 0.001f && maxElbowStep < 0.06f); + } +}