Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Body step 1: one-time T-pose calibration (prefs.json, forced gate) + True Body solver + shared ArmSolver

Browse Source 
- 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:
FoxyzandClaude Opus 5.5 committed 2026-10-02 05:47:20 +03:00
1 parent 3a990c6dbe
commit 81a9b1fca2
15 files changed
+994 -106
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

scripts/PhysicsArms.cs
+39 -96
View File
@@ -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<XRCamera3D>("../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;
}
}