FoxyzandClaude Opus 5.5
e821728fba
- Look-down flip: body facing from BodyMath.HeadFacing (visor + crown), robust past vertical; stick direction uses it too; crouch lean pitch measured against the facing. - Landing: falling legs touch down only within their reach, so leg strength stops the body below its stance (dip scales with impact, big falls fold the legs). Rise eased so gravity can stop it at the stance; legs untuck speed-limited. Fixed the vertical play-space correction leaking into ground control's feed-forward (a bounce). - Jump on right A: countermovement dip + leg push over the stroke (crouch = higher). - Stick crouch on right stick Y (RagdollDriver.StickCrouch: virtual floor offset). - Menu summon on left X only. - XR origin drawn with the bodies' physics interpolation (camera/body desync). - RagdollSpike: landing/fall/jump/look-down/stick-crouch phases, `-- only=` filter. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
290 lines
14 KiB
C#
290 lines
14 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 MaxHeadPitch = 2.4f; // rad; chin-to-chest looking at your feet goes past 90°
|
||
// Crawl: when the head gets so low that the hips would go through the floor, the trunk tips
|
||
// further (pivoting at the hips, hips kicking back) up to nearly horizontal (Nimso: lower = crawl).
|
||
public const float CrawlLeanMax = 1.45f; // rad (~83°)
|
||
public const float HipMinHeight = 0.15f; // hip joint above the floor when crawling
|
||
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 Vector3 PelvisTarget; // un-lagged pelvis (the ragdoll's ground control adds its own lag)
|
||
public Basis ChestBasis = Basis.Identity;
|
||
public Basis PelvisBasis = Basis.Identity;
|
||
public Basis YawBasis = Basis.Identity;
|
||
public Basis HeadBasis = Basis.Identity;
|
||
public float Lean { get; private set; }
|
||
public readonly Vector3[] Pole = new Vector3[2]; // per arm, see ArmSolver.Pole
|
||
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;
|
||
|
||
// A discrete move of the whole player (teleport, snap turn): carry the lagging state along
|
||
// instead of letting it swing across the gap.
|
||
public void Shift(Transform3D delta)
|
||
{
|
||
if (!Initialized)
|
||
return;
|
||
Pelvis = delta * Pelvis;
|
||
_pelvisVelocity = delta.Basis * _pelvisVelocity;
|
||
Vector3 f = delta.Basis * new Basis(Vector3.Up, _yaw) * Vector3.Forward;
|
||
_yaw = Mathf.Atan2(-f.X, -f.Z);
|
||
}
|
||
|
||
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();
|
||
HeadBasis = headBasis;
|
||
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).
|
||
// Facing from BodyMath.HeadFacing: looking down past vertical must not turn the body around.
|
||
Vector3 headFwd = -headBasis.Z;
|
||
Vector3 flatFwd = BodyMath.HeadFacing(headBasis);
|
||
if (flatFwd == Vector3.Zero)
|
||
flatFwd = new Basis(Vector3.Up, _yaw) * Vector3.Forward;
|
||
float hmdYaw = BodyMath.YawOf(flatFwd);
|
||
float targetYaw = hmdYaw;
|
||
Vector3 toHands = (WristTarget[0] + WristTarget[1]) * 0.5f - Eye;
|
||
toHands.Y = 0f;
|
||
if (toHands.LengthSquared() > 1e-4f)
|
||
{
|
||
float handsYaw = BodyMath.YawOf(toHands);
|
||
float forwardness = Mathf.Clamp(toHands.Dot(flatFwd) / 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);
|
||
YawBasis = yawBasis;
|
||
|
||
// 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);
|
||
// Pitch against the facing, so it keeps growing past vertical (asin peaks at 90°).
|
||
float headPitchDown = Mathf.Clamp(Mathf.Atan2(-headFwd.Y, headFwd.Dot(flatFwd)), 0f, MaxHeadPitch);
|
||
float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest;
|
||
float trunk = TrunkRatio * p.Stature;
|
||
float neckLen = Mathf.Max(0.05f, p.ShoulderDrop + EyeToNeckPivot.Y - NeckBaseAboveShoulders);
|
||
Vector3 neckDir = Vector3.Up;
|
||
for (int iter = 0; iter < 3; iter++)
|
||
{
|
||
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.
|
||
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);
|
||
float room = (UpperChest.Y - floorY - HipMinHeight) / trunk;
|
||
float crawl = Mathf.Acos(Mathf.Clamp(room, Mathf.Cos(CrawlLeanMax), 1f));
|
||
if (crawl <= lean + 1e-3f)
|
||
break;
|
||
lean = crawl;
|
||
}
|
||
Lean = lean;
|
||
|
||
// 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);
|
||
PelvisTarget = pelvisTarget;
|
||
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;
|
||
Pole[side] = _arms[side].Pole;
|
||
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();
|
||
// Set by PlayerRig, which solves at the right point of its own tick (after moving the origin).
|
||
public bool ExternallySolved;
|
||
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 || ExternallySolved)
|
||
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);
|
||
}
|
||
}
|