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:
FoxyzandClaude Opus 5.5 committed 2026-10-02 05:47:20 +03:00
1 parent 3a990c6dbe
commit 81a9b1fca2
15 files changed
+994 -106

No files matched your search

+102
View File
@@ -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();
}
}