FoxyzandClaude Opus 5.5
81a9b1fca2
- 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>
248 lines
12 KiB
C#
248 lines
12 KiB
C#
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<MeshInstance3D> _bones = new();
|
||
private readonly System.Collections.Generic.List<MeshInstance3D> _joints = new();
|
||
|
||
public TrueBodySolver Solver { get; } = new();
|
||
public bool HasPose => Solver.Initialized;
|
||
|
||
public override void _Ready()
|
||
{
|
||
_origin = GetNode<XROrigin3D>("../XROrigin3D");
|
||
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
||
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||
_right = GetNode<XRController3D>("../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);
|
||
}
|
||
}
|