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,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();
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user