diff --git a/scripts/BodyMath.cs b/scripts/BodyMath.cs index 5c2a972..dad8fdd 100644 --- a/scripts/BodyMath.cs +++ b/scripts/BodyMath.cs @@ -3,36 +3,56 @@ using Godot; // Shared math for the chase-driven player bodies (hands, torso, arm segments). public static class BodyMath { - // Shortest-arc rotation taking `current` to `target`, as axis * angle (radians), world space. - // Never use Quaternion.GetAngle()/GetAxis() for servo errors: GetAngle() is 2*acos(W) and - // returns NaN when float rounding puts W at 1.0000001 (the Stage-1 chest collapse), and - // GetAxis() returns the raw un-normalized xyz once W > 1 - 1e-6 (servo gain collapses - // ~1000x inside 0.16 deg). atan2 is exact near zero and can't leave its domain. - public static Vector3 RotationError(Quaternion target, Quaternion current) - { - Quaternion q = target * current.Inverse(); - if (q.W < 0f) - q = new Quaternion(-q.X, -q.Y, -q.Z, -q.W); - Vector3 v = new(q.X, q.Y, q.Z); - float s = v.Length(); - if (s < 1e-9f) - return Vector3.Zero; - return v * (2f * Mathf.Atan2(s, q.W) / s); - } + // Shortest-arc rotation taking `current` to `target`, as axis * angle (radians), world space. + // Never use Quaternion.GetAngle()/GetAxis() for servo errors: GetAngle() is 2*acos(W) and + // returns NaN when float rounding puts W at 1.0000001 (the Stage-1 chest collapse), and + // GetAxis() returns the raw un-normalized xyz once W > 1 - 1e-6 (servo gain collapses + // ~1000x inside 0.16 deg). atan2 is exact near zero and can't leave its domain. + public static Vector3 RotationError(Quaternion target, Quaternion current) + { + Quaternion q = target * current.Inverse(); + if (q.W < 0f) + q = new Quaternion(-q.X, -q.Y, -q.Z, -q.W); + Vector3 v = new(q.X, q.Y, q.Z); + float s = v.Length(); + if (s < 1e-9f) + return Vector3.Zero; + return v * (2f * Mathf.Atan2(s, q.W) / s); + } - public static bool IsFinite(RigidBody3D body) => - body.GlobalTransform.IsFinite() && body.LinearVelocity.IsFinite() && body.AngularVelocity.IsFinite(); + // The horizontal direction a head faces, robust past vertical. The visor (-Z) projected flat + // flips 180° once you look down (or up) beyond vertical — chin to chest while crouching, or + // looking at your own feet — and everything built on it (torso yaw, stick direction) turned + // around. Looking down, the top of the head (+Y) points the way you face; looking up it points + // back. So: when the flat visor disagrees with the flat crown, the head is past vertical and the + // visor is reversed; near vertical the crown alone carries the direction. Zero if undefined. + public static Vector3 HeadFacing(Basis head) + { + Vector3 forward = -head.Z, up = head.Y; + Vector3 visor = new(forward.X, 0f, forward.Z); + Vector3 crown = new Vector3(up.X, 0f, up.Z) * -forward.Y; + if (visor.Dot(crown) < 0f) + visor = -visor; + Vector3 f = visor + crown * 0.5f; + return f.LengthSquared() > 1e-6f ? f.Normalized() : Vector3.Zero; + } - // Escape hatch: a non-finite body is unrecoverable for the solver and floods the renderer - // with !v.is_finite() errors — put it back on its target, motionless. Returns true if reset. - public static bool ResetIfNonFinite(RigidBody3D body, Transform3D target, string who) - { - if (IsFinite(body)) - return false; - body.GlobalTransform = target; - body.LinearVelocity = Vector3.Zero; - body.AngularVelocity = Vector3.Zero; - GD.PushWarning($"{who}: non-finite body state, reset to target"); - return true; - } + // Yaw (about +Y, Godot convention: 0 = facing -Z) of a horizontal direction. + public static float YawOf(Vector3 flat) => Mathf.Atan2(-flat.X, -flat.Z); + + public static bool IsFinite(RigidBody3D body) => + body.GlobalTransform.IsFinite() && body.LinearVelocity.IsFinite() && body.AngularVelocity.IsFinite(); + + // Escape hatch: a non-finite body is unrecoverable for the solver and floods the renderer + // with !v.is_finite() errors — put it back on its target, motionless. Returns true if reset. + public static bool ResetIfNonFinite(RigidBody3D body, Transform3D target, string who) + { + if (IsFinite(body)) + return false; + body.GlobalTransform = target; + body.LinearVelocity = Vector3.Zero; + body.AngularVelocity = Vector3.Zero; + GD.PushWarning($"{who}: non-finite body state, reset to target"); + return true; + } } diff --git a/scripts/PlayerRagdoll.cs b/scripts/PlayerRagdoll.cs index db650fb..913b90b 100644 --- a/scripts/PlayerRagdoll.cs +++ b/scripts/PlayerRagdoll.cs @@ -22,987 +22,1087 @@ using System.Collections.Generic; // All motors are PDs written as target velocities each tick; the force/torque limits are the strength. public partial class PlayerRagdoll : Node3D { - public const float BodyMass = 60f; // Nimso's character is 60 kg - // Muscles: PD gain (1/s), max commanded spin (rad/s), torque cap (N·m). - public const float SpineKp = 15f, SpineTorque = 400f; - public const float NeckKp = 20f, NeckTorque = 60f; - // Asymmetric by direction (Nimso; human strength tables): pulling the arm DOWN (lats/back — - // pull-ups, hanging, climbing) is roughly twice as strong as raising it (front delts — a weight - // held out front), and elbow flexion (biceps) beats extension. - public const float ShoulderKp = 20f, ShoulderTorque = 35f, ShoulderPullTorque = 100f; - public const float ElbowKp = 20f, ElbowTorque = 45f, ElbowFlexTorque = 70f; - public const float MuscleMaxSpin = 15f; - // Feed-forwards are derivatives of tracking: low-pass them or tracking jitter (±1–2 mm per - // sample) becomes hundreds of newtons of buzz (RagdollSpike REAL input mode). - public const float MuscleFeedForwardTau = 0.05f; // s - public const float GroundFeedForwardTau = 0.12f; // s - public const float GirdleKp = 30f, GirdleForce = 2000f; // shoulder drive, linear part - // Hand guide (chest→hand drive): pulls the hand onto the controller; capped so the arm, - // not the guide, carries real loads. - public const float HandGuideKp = 40f, HandGuideForce = 40f, HandGuideMaxSpeed = 10f; - // STRUCTURAL strength: force along the arm (straight arm pressing on a ledge, hanging from a - // bar, carrying a bucket at your side) goes through bone and is strong; force across the arm - // needs muscle torque and stays at HandGuideForce. Cap = lerp(base, structural, alignment). - // Two arms ≈ 1.2 × body weight → two hands mantle/pull up, one can't (Nimso). - public const float HandStructuralForce = 400f; - public const float StructuralDeadband = 0.03f; - // Nimso's one global strength value: scales every muscle and the hand guides (menu slider). - public static float Strength = 1f; - public const float HandGuideAngularKp = 40f, HandGuideTorque = 25f; - // Ground control (world-space support on the pelvis, only while grounded). - public const float GroundFrequency = 4f; // Hz - public const float GroundDamping = 1f; - public const float LegStrength = 4.5f; // max support, × body weight (real legs brake a landing at 3–5 g; 2× sank ~0.4 m) - public const float GroundFriction = 1f; // horizontal force ≤ this × support - public const float GroundProbeExtra = 0.3f; // ground counts while within hip height + this - public const float MaxStepHeight = 0.35f; - public const float MaxRiseSpeed = 1.2f; // m/s the legs lift the body toward its stance - public const float MaxSlopeDegrees = 35f; // steeper ground gives no support (Nimso: "slopes beyond an angle can't be run up") // higher ground under the hips isn't stood on unless the body got up there - public const float BracedAirControl = 150f; // N of horizontal hip carry while airborne but braced on the hands (mantling) - public const float CrawlCarry = 300f; - public const float BlockedPush = 150f; // N horizontal leg push while the hips are held back // N: the torso lies on the floor in a crawl; knees/hands would push (not simulated) - public const float TorsoFriction = 0.3f; // clothing on floor: lets a crawl slide - public const float LimbFriction = 0.25f; - // Lower-body collider: a box from the hips down to MaxStepHeight above the feet, so knee-high - // obstacles block walking (no walking over tables) while lower steps pass under it. It slides - // up into the torso when crouching and switches off when crawling (Nimso: legs don't snag). - public const float LegsMass = 6f; - public const float LegsKp = 30f, LegsForce = 3000f, LegsTorque = 400f; - public const float LegsOffAbove = 0.25f; // collider disabled once slid this far above the hips - public const float MantleLift = 0.03f; // braced + hips this far above the support goal = being lifted (legs tuck) - public const float UntuckDelay = 0.3f; // s the tuck must be unneeded before the collider returns - // Balance: an implicit torque-capped motor from a world anchor to the pelvis (explicit torques - // let the arms' reaction torques set the whole trunk swinging). - public const float BalanceKp = 10f; - public const float BalanceTorque = 800f; - public const float AirBalanceShare = 0.3f; - // Play-space reconciliation. - public const float CameraRadius = 0.03f; // sphere swept from the head's eye to the camera - public const float FreeSlack = 0.12f; // camera leading the body's eye by more than this = body held back: stick stops pushing that way - public const float ViewDeadzone = 0.02f; // m the head may lead the body before the view is eased back onto it - public const float ViewFollowTau = 0.04f; // s - public const float MaxViewLead = 0.04f; // hard: the head box reaches 2 cm in front of the eye - public const float FloorRecenterTau = 1f; // s - public const float HeadGuideFrequency = 5f; // Hz - public const float HeadGuideMass = 15f; // kg effective (head + part of the trunk) - public const float HeadGuideForce = 250f; // N - public const float ResetDistance = 1.5f; // pelvis this far from its target → snap the whole body to the pose - // Geometry. - public const float LumbarFraction = 0.35f; // lumbar joint height above the hips, × trunk - public const float PelvisLeanShare = 0.5f; // share of the crouch lean taken by the hips - public static readonly Vector3 HeadCenterFromEye = new(0f, -0.005f, 0.08f); // crown ~10 cm above the eyes, chin ~11 cm below (was 12.5 / 8.5) - public static readonly Vector3 HeadSize = new(0.16f, 0.21f, 0.2f); - public static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f); - public const float WristOffset = TrueBodySolver.WristOffset; - public const float ElbowMaxFlex = 2.6f; // rad (~150°) - // Sign of HingeJoint3D's angle/motor for a positive rotation of the child about the joint's - // +Z (measured by RagdollSpike's "hinge sign" phase). - public static float HingeSign = 1f; - - // Collision layers (AGENTS.md): 3 body, 4 hands, 5 arms. - private const uint BodyLayer = 0b100, HandLayer = 0b1000, ArmLayer = 0b10000; - private const uint WorldMask = 0b1; - - private sealed class Ball - { - public Generic6DofJoint3D Joint; - public RigidBody3D Parent, Child; - public Quaternion FrameA, FrameB; // joint frame relative to each body - public float Kp; - public Quaternion PrevTarget; - public bool HasPrev; - public Vector3 Ff; - } - - private sealed class Drive - { - public Generic6DofJoint3D Joint; - public Quaternion FrameA, FrameB; - public Vector3 AnchorA, AnchorB; // anchor point, parent-local / child-local - public Quaternion PrevTarget; - public bool HasPrev; - public Vector3 Ff; - } - - private sealed class Arm - { - public RigidBody3D Upper, Fore, Hand; - public Ball Wrist; - public HingeJoint3D Elbow; - public Drive Shoulder, Guide; - public float UpperLen, ForeLen; - public float PrevFlexTarget = -1f; - public float FlexFf; - public float GuideCap = HandGuideForce; - public float ShoulderCap = ShoulderTorque, ElbowCap = ElbowTorque; - public PhysicsBody3D Held; - } - - public RigidBody3D Pelvis { get; private set; } - public RigidBody3D Chest { get; private set; } - public RigidBody3D Head { get; private set; } - public bool Built { get; private set; } - public bool Grounded { get; private set; } - public float GroundY => _footY; - public bool Braced { get; private set; } // a hand touches or holds static world - public bool LegsSolid => _legsSolid; - public bool Lifted => _lifted; - // Horizontal direction in which the body is held back from the camera (zero if not): the stick - // must not keep moving the play space that way. - public Vector3 HeldBack { get; private set; } - // Really climbing: a hand holds static world, or the arms are lifting the body (mantle). - public bool Climbing => Built && (_arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D || _lifted); - public RigidBody3D Legs { get; private set; } - public Vector3 Eye => Head.GlobalTransform * _eyeLocal; - public BodyProfile Profile { get; private set; } - public int Snaps { get; private set; } // safety resets (NaN / too far) — should stay 0 - - private readonly Arm[] _arms = { new(), new() }; - private readonly List _bodies = new(); - private Ball _spine, _neck; - private Vector3 _neckBaseToEye; // head frame, canonical (neck straight) - private float _trunk, _lumbar, _neckLen; - private Vector3 _eyeLocal; - private Vector3 _pelvisCenterLocal, _chestCenterLocal, _headCenterLocal; // from each body's proximal joint, in its basis - private readonly Dictionary _targets = new(); - private readonly Vector3[] _girdle = new Vector3[2]; // True Body shoulder offset, chest frame - private float _footY; - private float _supportGoalY; - private RigidBody3D _groundBody; - private Vector3 _groundPoint; - private float _tuckFree, _dt; - private PhysicsShapeQueryParameters3D _legsQuery; - - private bool LegsSpaceFree(Vector3 center, Basis basis) - { - _legsQuery ??= new PhysicsShapeQueryParameters3D { Shape = Legs.GetChild(0).Shape, CollisionMask = WorldMask }; - _legsQuery.Transform = new Transform3D(basis, center); - return GetWorld3D().DirectSpaceState.IntersectShape(_legsQuery, 1).Count == 0; - } - private Vector3 _goalVelocity; - - private static float LowPass(float dt, float tau) => 1f - Mathf.Exp(-dt / tau); - private bool _lifted; - private float _legsHeight; - private Drive _legsDrive; - private bool _legsSolid = true; - private ProceduralLegs _legVisuals; - private float _appliedStrength = 1f; - private Vector3 _pelvisTargetPrev; - private bool _havePelvisPrev; - - public RigidBody3D Hand(int side) => Built ? _arms[side].Hand : null; - public RigidBody3D UpperArm(int side) => Built ? _arms[side].Upper : null; - public RigidBody3D Forearm(int side) => Built ? _arms[side].Fore : null; - public IReadOnlyList Bodies => _bodies; - - // ---------------------------------------------------------------- build - - public void Build(BodyProfile p, TrueBodySolver pose, Transform3D left, Transform3D right) - { - Clear(); - Profile = p; - _trunk = TrueBodySolver.TrunkRatio * p.Stature; - _lumbar = LumbarFraction * _trunk; - _neckLen = Mathf.Max(0.05f, p.ShoulderDrop + TrueBodySolver.EyeToNeckPivot.Y - TrueBodySolver.NeckBaseAboveShoulders); - float m = BodyMass; - float s = p.Stature / 1.75f; - - // Canonical pose (standing, level head, arms straight down-forward-out) in frame C at the - // current pelvis: joint cones/twists are centred on it and the elbow hinge's zero is a - // straight arm. The body is then moved onto the live pose. - Transform3D c = new(pose.YawBasis, pose.PelvisTarget); - Vector3 hip = Vector3.Zero; - Vector3 lumbar = new(0f, _lumbar, 0f); - Vector3 upperChest = new(0f, _trunk, 0f); - Vector3 neckBase = upperChest + new Vector3(0f, TrueBodySolver.NeckBaseAboveShoulders, - -(TrueBodySolver.ShouldersBehindEyes - TrueBodySolver.EyeToNeckPivot.Z)); - Vector3 neckPivot = neckBase + new Vector3(0f, _neckLen, 0f); - _eyeLocal = -TrueBodySolver.EyeToNeckPivot; // head body origin is set to its centre below - Vector3 headCenterFromPivot = _eyeLocal + HeadCenterFromEye; - - Vector3 pelvisSize = new(0.17f * p.Stature, _lumbar + 0.06f, 0.18f * s); - _pelvisCenterLocal = new Vector3(0f, _lumbar * 0.5f - 0.03f, 0f); - float chestHeight = _trunk - _lumbar + TrueBodySolver.NeckBaseAboveShoulders; - Vector3 chestSize = new(2f * p.ShoulderHalfWidth * 0.75f, chestHeight, 0.2f * s); - _chestCenterLocal = new Vector3(0f, chestHeight * 0.5f, 0f); - _headCenterLocal = headCenterFromPivot; - _eyeLocal -= headCenterFromPivot; // eye relative to the head body's centre - - Color skin = new(0.85f, 0.72f, 0.6f), shirt = new(0.3f, 0.45f, 0.7f), pants = new(0.25f, 0.27f, 0.32f); - Pelvis = MakeBody("Pelvis", new BoxShape3D { Size = pelvisSize }, (0.142f + 0.139f + 0.322f) * m - LegsMass, BodyLayer, WorldMask, pants, - c * new Transform3D(Basis.Identity, hip + _pelvisCenterLocal), friction: TorsoFriction); - float standingHip = pose.PelvisTarget.Y - (pose.Eye.Y - p.EyeHeight); - _legsHeight = Mathf.Max(0.2f, standingHip - MaxStepHeight); - Legs = MakeBody("Legs", new BoxShape3D { Size = new Vector3(0.28f * s, _legsHeight, 0.2f * s) }, LegsMass, BodyLayer, WorldMask, pants, - c * new Transform3D(Basis.Identity, hip - new Vector3(0f, _legsHeight * 0.5f, 0f)), visible: false); - // Explicit inertia: the collider is switched off when tucked, and a body without an - // enabled shape has no inertia — its 400 N·m drive then tumbled the whole pelvis - // (owner 2.1 "body spinning" when crouched). - Legs.Inertia = new Vector3(_legsHeight * _legsHeight + 0.04f, 0.28f * 0.28f + 0.04f, _legsHeight * _legsHeight + 0.08f) * (LegsMass / 12f); - _footY = pose.Eye.Y - p.EyeHeight; - Chest = MakeBody("Chest", new BoxShape3D { Size = chestSize }, 0.216f * m, BodyLayer, WorldMask, shirt, - c * new Transform3D(Basis.Identity, lumbar + _chestCenterLocal), friction: TorsoFriction); - Head = MakeBody("Head", new BoxShape3D { Size = HeadSize * s }, 0.081f * m, BodyLayer, WorldMask, skin, - c * new Transform3D(Basis.Identity, neckPivot + headCenterFromPivot), friction: TorsoFriction); - // The neck: a box hanging under the head, same body. - Vector3 neckCenter = (neckBase + neckPivot) * 0.5f - (neckPivot + headCenterFromPivot); - Head.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter }); - MeshInstance3D neckMesh = new() { Mesh = new BoxMesh { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter }; - neckMesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = skin, Roughness = 0.6f }); - Head.AddChild(neckMesh); - _neckBaseToEye = neckPivot - TrueBodySolver.EyeToNeckPivot - neckBase; - - // World anchor for the balance motor (no shape, linear motor force 0 = position free). - StaticBody3D anchor = new() { Name = "BalanceAnchor", CollisionLayer = 0, CollisionMask = 0 }; - AddChild(anchor); - _balance = MakeDrive(anchor, Pelvis, Pelvis.GlobalTransform, 0f, BalanceTorque); - _legsDrive = MakeDrive(Pelvis, Legs, c * new Transform3D(Basis.Identity, hip), LegsForce, LegsTorque); - _legVisuals = new ProceduralLegs { Name = "LegVisuals" }; - AddChild(_legVisuals); - _legVisuals.Setup(Pelvis, -_pelvisCenterLocal, p.Stature, pants, new Color(0.15f, 0.15f, 0.17f)); - _balanceCap = BalanceTorque; - - _spine = MakeBall(Pelvis, Chest, c * lumbar, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(60f), Mathf.DegToRad(35f), SpineKp, SpineTorque); - _neck = MakeBall(Chest, Head, c * neckBase, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(75f), Mathf.DegToRad(85f), NeckKp, NeckTorque); - - for (int side = 0; side < 2; side++) - { - Arm a = _arms[side]; - float sign = side == 0 ? -1f : 1f; - a.UpperLen = p.UpperArm; - a.ForeLen = p.Forearm; - Vector3 shoulder = upperChest + new Vector3(sign * p.ShoulderHalfWidth, 0f, 0f); - Vector3 dir = new Vector3(sign * 0.42f, -0.74f, -0.47f).Normalized(); - Vector3 reference = Vector3.Down * 2f + Vector3.Back * 0.8f + new Vector3(sign * 0.3f, 0f, 0f); - Vector3 pole = (reference - dir * reference.Dot(dir)).Normalized(); - Vector3 hinge = pole.Cross(dir).Normalized(); - Basis bone = BoneBasis(dir, hinge); - Vector3 elbow = shoulder + dir * a.UpperLen; - Vector3 wrist = elbow + dir * a.ForeLen; - Vector3 handX = (Vector3.Right - dir * dir.X).Normalized(); - Basis handBasis = new(handX, (-dir).Cross(handX), -dir); - Vector3 handOrigin = wrist + dir * WristOffset; - string n = side == 0 ? "Left" : "Right"; - - a.Upper = MakeBody(n + "UpperArm", new BoxShape3D { Size = new Vector3(0.085f * s, a.UpperLen, 0.085f * s) }, 0.028f * m, ArmLayer, WorldMask, shirt, - c * new Transform3D(bone, shoulder + dir * (a.UpperLen * 0.5f)), friction: LimbFriction); - a.Fore = MakeBody(n + "Forearm", new BoxShape3D { Size = new Vector3(0.07f * s, a.ForeLen - 0.01f, 0.07f * s) }, 0.016f * m, ArmLayer, WorldMask, skin, - c * new Transform3D(bone, elbow + dir * (a.ForeLen * 0.5f)), friction: LimbFriction); - a.Hand = MakeBody(n + "Hand", new BoxShape3D { Size = HandSize }, 0.006f * m, HandLayer, WorldMask | HandLayer, - side == 0 ? new Color(0.9f, 0.3f, 0.35f) : new Color(0.3f, 0.5f, 0.9f), c * new Transform3D(handBasis, handOrigin), friction: LimbFriction); - a.Hand.ContinuousCd = true; - a.Hand.ContactMonitor = true; - a.Hand.MaxContactsReported = 8; - - a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(Basis.Identity, shoulder), GirdleForce, ShoulderTorque); - a.Elbow = MakeHinge(a.Upper, a.Fore, c * elbow, c.Basis * new Basis(dir, hinge.Cross(dir), hinge)); - a.Wrist = MakeBall(a.Fore, a.Hand, c * wrist, c.Basis * TwistAlong(dir), Mathf.DegToRad(80f), Mathf.DegToRad(90f), 0f, 0f); - a.Guide = MakeDrive(Chest, a.Hand, c * new Transform3D(handBasis, handOrigin), HandGuideForce, HandGuideTorque); - } - - Built = true; - _havePelvisPrev = false; - _appliedStrength = 1f; - _legsSolid = true; - ApplyStrength(); - SnapToPose(pose, left, right); - GD.Print($"PlayerRagdoll: built for stature {p.Stature:F2} m (arm {p.UpperArm:F3}+{p.Forearm:F3} m, {_bodies.Count} bodies)"); - } - - public void Clear() - { - foreach (Node child in GetChildren()) - child.QueueFree(); - foreach (Node child in GetChildren()) - RemoveChild(child); - _bodies.Clear(); - _targets.Clear(); - for (int i = 0; i < 2; i++) - _arms[i] = new Arm(); - Built = false; - } - - // Twist axis (joint X) along `dir`. - private static Basis TwistAlong(Vector3 dir) - { - Vector3 helper = Mathf.Abs(dir.Y) < 0.9f ? Vector3.Up : Vector3.Right; - Vector3 z = dir.Cross(helper).Normalized(); - return new Basis(dir, z.Cross(dir), z); - } - - // Segment basis: Y along the bone, X = elbow hinge axis. - private static Basis BoneBasis(Vector3 boneDir, Vector3 hinge) - { - Vector3 x = (hinge - boneDir * hinge.Dot(boneDir)).Normalized(); - return new Basis(x, boneDir, x.Cross(boneDir)); - } - - // NOTE (Box3D wrapper): friction is read only when the body's shapes are created — the material - // must be set before AddChild (setting it later silently does nothing). - private RigidBody3D MakeBody(string name, Shape3D shape, float mass, uint layer, uint mask, Color color, Transform3D at, bool visible = true, float friction = 1f) - { - RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at, ContactMonitor = true, MaxContactsReported = 4, - PhysicsMaterialOverride = new PhysicsMaterial { Friction = friction } }; - body.AddChild(new CollisionShape3D { Shape = shape }); - if (shape is BoxShape3D box && layer != 0 && visible) - { - MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = box.Size } }; - mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f }); - body.AddChild(mesh); - } - AddChild(body); - _bodies.Add(body); - return body; - } - - private static Quaternion Rot(Node3D n) => n.GlobalTransform.Basis.Orthonormalized().GetRotationQuaternion(); - - // Configure BEFORE AddChild (the node pushes everything to the server on enter-tree). - private Ball MakeBall(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame, float cone, float twist, float kp, float torque) - { - Generic6DofJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; - j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -twist); - j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, twist); - foreach (var set in new System.Action[] { j.SetParamY, j.SetParamZ }) - { - set(Generic6DofJoint3D.Param.AngularLowerLimit, -cone); - set(Generic6DofJoint3D.Param.AngularUpperLimit, cone); - } - if (torque > 0f) - { - j.SetFlagX(Generic6DofJoint3D.Flag.EnableMotor, true); - j.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, true); - j.SetFlagZ(Generic6DofJoint3D.Flag.EnableMotor, true); - j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); - j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); - j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); - } - AddChild(j); - Quaternion jq = frame.Orthonormalized().GetRotationQuaternion(); - return new Ball { Joint = j, Parent = parent, Child = child, FrameA = Rot(parent).Inverse() * jq, FrameB = Rot(child).Inverse() * jq, Kp = kp }; - } - - private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque) - { - Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; - foreach (var flag in new System.Action[] { j.SetFlagX, j.SetFlagY, j.SetFlagZ }) - { - flag(Generic6DofJoint3D.Flag.EnableLinearLimit, false); - flag(Generic6DofJoint3D.Flag.EnableAngularLimit, false); - flag(Generic6DofJoint3D.Flag.EnableLinearMotor, true); - flag(Generic6DofJoint3D.Flag.EnableMotor, true); - } - foreach (var param in new System.Action[] { j.SetParamX, j.SetParamY, j.SetParamZ }) - { - param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force); - param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); - } - AddChild(j); - Quaternion jq = at.Basis.Orthonormalized().GetRotationQuaternion(); - return new Drive - { - Joint = j, - FrameA = Rot(parent).Inverse() * jq, - FrameB = Rot(child).Inverse() * jq, - AnchorA = parent.GlobalTransform.AffineInverse() * at.Origin, - AnchorB = child.GlobalTransform.AffineInverse() * at.Origin, - }; - } - - private HingeJoint3D MakeHinge(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame) - { - HingeJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; - float lo = HingeSign > 0f ? 0f : -ElbowMaxFlex; - float hi = HingeSign > 0f ? ElbowMaxFlex : 0f; - j.SetFlag(HingeJoint3D.Flag.UseLimit, true); - j.SetParam(HingeJoint3D.Param.LimitLower, lo); - j.SetParam(HingeJoint3D.Param.LimitUpper, hi); - j.SetFlag(HingeJoint3D.Flag.EnableMotor, true); - j.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque); - AddChild(j); - return j; - } - - // ---------------------------------------------------------------- pose targets - - // World transform every body should have in the True Body pose. The trunk is rebuilt top-down - // through the physical chain (upper chest → lumbar → hips) so a leaning chest doesn't drag - // the head off the camera; the hips take part of the crouch lean (hip hinge). - private void ComputeTargets(TrueBodySolver s, Transform3D left, Transform3D right, bool physicalArms = false, float dt = 0f) - { - Basis chest = s.ChestBasis; - // Hips take half of a crouch lean, all of it in a crawl (keeps them off the floor). - float share = Mathf.Lerp(PelvisLeanShare, 1f, Mathf.Clamp((s.Lean - TrueBodySolver.ChestLeanMax) / 0.5f, 0f, 1f)); - Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * share); - Vector3 lumbar = s.UpperChest - chest.Y * (_trunk - _lumbar); - Vector3 hip = lumbar - pelvis.Y * _lumbar; - _targets[Pelvis] = new Transform3D(pelvis, hip + pelvis * _pelvisCenterLocal); - _targets[Chest] = new Transform3D(chest, lumbar + chest * _chestCenterLocal); - _targets[Legs] = new Transform3D(s.YawBasis, hip - Vector3.Up * (_legsHeight * 0.5f)); // snap pose only; LegsDrive steps it - // Head + neck pivot at the neck base: orient like the camera, tilted by the small extra - // rotation that puts the body's eye on the camera (the True Body bends the neck at two points). - Vector3 eyeDir = s.HeadBasis * _neckBaseToEye; - Vector3 wantDir = s.Eye - s.NeckBase; - Quaternion tilt = eyeDir.LengthSquared() > 1e-8f && wantDir.LengthSquared() > 1e-8f - ? new Quaternion(eyeDir.Normalized(), wantDir.Normalized()) : Quaternion.Identity; - Basis head = new Basis(tilt) * s.HeadBasis; - _targets[Head] = new Transform3D(head, s.NeckBase + head * (_neckBaseToEye - _eyeLocal)); - - for (int side = 0; side < 2; side++) - { - Arm a = _arms[side]; - Vector3 sh = s.Shoulder[side], el = s.Elbow[side], wr = s.Wrist[side]; - Vector3 armDir = wr - sh; - armDir = armDir.LengthSquared() > 1e-8f ? armDir.Normalized() : chest * Vector3.Down; - Vector3 hinge = s.Pole[side].Cross(armDir); - hinge = hinge.LengthSquared() > 1e-8f ? hinge.Normalized() : chest.X; - Vector3 up = (el - sh).Normalized(); - Vector3 fore = (wr - el).Normalized(); - _girdle[side] = chest.Transposed() * (sh - (s.UpperChest + chest.X * ((side == 0 ? -1f : 1f) * Profile.ShoulderHalfWidth))); - _targets[a.Upper] = new Transform3D(BoneBasis(up, hinge), sh + up * (a.UpperLen * 0.5f)); - _targets[a.Fore] = new Transform3D(BoneBasis(fore, hinge), el + fore * (a.ForeLen * 0.5f)); - _targets[a.Hand] = (side == 0 ? left : right).Orthonormalized(); - - if (physicalArms) - { - // Re-aim the arm from the PHYSICAL shoulder. Horizontally the body may be held away from - // the camera (pressed against a ledge, leaning over a table): the arm reaches for the - // real hand. Vertically a sagging body keeps its arm pose (the aim point moves with - // the shoulder), so straight real arms stay straight virtual arms — pillars that hold - // you up when crawling or press you up a ledge (the hand guide adds the rest). - // Only a horizontal TRANSLATION of the reach: solved in the chest TARGET frame (the - // muscles stay local). Aiming in the physical chest frame made the shoulder muscles - // feed chest rotation back into the chest — a slow twist that tracking jitter - // excited into "chest slipping out of control" (owner playtest 2.1). - Transform3D chestPhys = Chest.GlobalTransform.Orthonormalized(); - Vector3 shPhys = chestPhys * (a.Shoulder.AnchorA + _girdle[side]); - Vector3 held = shPhys - sh; - held.Y = 0f; - Transform3D ctrl = _targets[a.Hand]; - ArmSolver solver = _armSolvers[side]; - solver.Solve(sh, ctrl * new Vector3(0f, 0f, WristOffset) - held, chest, chest.X * (side == 0 ? -1f : 1f), - ctrl.Basis, a.UpperLen, a.ForeLen, dt); - Vector3 d = solver.Wrist - sh; - Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chest.X; - h = h.LengthSquared() > 1e-8f ? h.Normalized() : chest.X; - Vector3 u = (solver.Elbow - sh).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized(); - _targets[a.Upper] = new Transform3D(BoneBasis(u, h), sh + u * (a.UpperLen * 0.5f)); - _targets[a.Fore] = new Transform3D(BoneBasis(f, h), solver.Elbow + f * (a.ForeLen * 0.5f)); - } - } - } - - private readonly ArmSolver[] _armSolvers = { new(), new() }; - - public Transform3D Target(RigidBody3D body) => _targets.TryGetValue(body, out Transform3D t) ? t : body.GlobalTransform; - - // ---------------------------------------------------------------- per tick - - public void Step(TrueBodySolver s, Transform3D left, Transform3D right, Vector3 leftVelocity, Vector3 rightVelocity, float floorY, float dt) - { - if (!Built) - return; - _dt = dt; - _floorYForHead = floorY; - ComputeTargets(s, left, right, physicalArms: true, dt); - foreach (RigidBody3D b in _bodies) - { - if (BodyMath.ResetIfNonFinite(b, Target(b), "PlayerRagdoll")) - { - Snaps++; - SnapToPose(s, left, right); - return; - } - } - - Transform3D pelvisTarget = _targets[Pelvis]; - if (Pelvis.GlobalPosition.DistanceTo(pelvisTarget.Origin) > ResetDistance) - { - GD.Print("PlayerRagdoll: body too far from the pose, snapped to it"); - Snaps++; - SnapToPose(s, left, right); - return; - } - - Muscle(_spine, _targets[Pelvis].Basis, _targets[Chest].Basis, dt); - Muscle(_neck, _targets[Chest].Basis, _targets[Head].Basis, dt); - if (Strength != _appliedStrength) - ApplyStrength(); - Braced = false; - for (int side = 0; side < 2; side++) - { - Arm a = _arms[side]; - ShoulderDrive(a, side, dt); - ElbowMuscle(a, dt); - HandGuide(a, _targets[a.Hand], side == 0 ? leftVelocity : rightVelocity); - Braced |= IsBraced(a); - } - GroundControl(pelvisTarget, floorY, dt); - HeadGuide(s, dt); - LegsDrive(s); - _legVisuals.Tick(_footY, Grounded, !_legsSolid, s.YawBasis, dt); - } - - // Muscle = PD on the joint's relative rotation toward the relative rotation of the two TARGET - // bases (a local pose, not a world pin), written as the motor's target velocity in frame A. - // Feed-forward = the target's own relative spin, so a moving pose isn't chased with a lag of - // speed/Kp. - private static void Muscle(Ball b, Basis parentTarget, Basis childTarget, float dt) - { - Quaternion current = (Rot(b.Parent) * b.FrameA).Inverse() * (Rot(b.Child) * b.FrameB); - Quaternion target = (parentTarget.Orthonormalized().GetRotationQuaternion() * b.FrameA).Inverse() - * (childTarget.Orthonormalized().GetRotationQuaternion() * b.FrameB); - Vector3 raw = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; - b.Ff = b.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); - b.PrevTarget = target; - b.HasPrev = true; - Vector3 w = (b.Ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin); - SetAngularVelocity(b.Joint, w); - } - - private static void SetAngularVelocity(Generic6DofJoint3D j, Vector3 w) - { - j.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X); - j.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y); - j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z); - } - - private static void SetLinearVelocity(Generic6DofJoint3D j, Vector3 v) - { - j.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X); - j.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y); - j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z); - } - - // Elbow flexion (0 = straight) about the upper arm's hinge axis. - public float ElbowFlex(int side) - { - Arm a = _arms[side]; - Basis u = a.Upper.GlobalTransform.Basis.Orthonormalized(); - Vector3 f = a.Fore.GlobalTransform.Basis.Orthonormalized().Y; - return Mathf.Atan2(u.X.Dot(u.Y.Cross(f)), u.Y.Dot(f)); - } - - private void ElbowMuscle(Arm a, float dt) - { - int side = a == _arms[0] ? 0 : 1; - Vector3 up = _targets[a.Upper].Basis.Y, fore = _targets[a.Fore].Basis.Y; - float target = Mathf.Acos(Mathf.Clamp(up.Dot(fore), -1f, 1f)); - float raw = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f; - a.FlexFf = Mathf.Lerp(a.FlexFf, raw, LowPass(dt, MuscleFeedForwardTau)); - a.PrevFlexTarget = target; - float w = Mathf.Clamp(a.FlexFf + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin); - a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w); - float cap = (w > 0f ? ElbowFlexTorque : ElbowTorque) * Mathf.Max(0.05f, Strength); - if (Mathf.Abs(cap - a.ElbowCap) > 0.5f) - { - a.ElbowCap = cap; - a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, cap); - } - } - - // Head guide (Boneworks/BIMOS drive the head to the HMD): while standing on the ground, a force- - // capped PD pulls the character's eye onto the camera, so voluntary head motion (squats, leans, - // quick looks) doesn't leave the body lagging — a lag the riding view would otherwise turn into - // a permanent play-space offset. Off while airborne/climbing (no levitating by the head) or - // bracing on the hands (mantling, pushing off). - private void HeadGuide(TrueBodySolver s, float dt) - { - Vector3 goal = s.Eye + Vector3.Up * (_footY - _floorYForHead); - Vector3 raw = _haveHeadPrev && dt > 0f ? (goal - _headGoalPrev) / dt : Vector3.Zero; - _headGoalPrev = goal; - _haveHeadPrev = true; - if (raw.Length() > 10f) - raw = Vector3.Zero; - _headGoalVelocity = _headGoalVelocity.Lerp(raw, LowPass(dt, GroundFeedForwardTau)); - // Only on your feet: hands bracing (mantling, pushing yourself) or crawling move the body by - // other means, and the view simply follows it. - if (!Grounded || Climbing || Braced) - return; - float w = Mathf.Tau * HeadGuideFrequency; - Vector3 force = ((goal - Eye) * (w * w) + (_headGoalVelocity - Head.LinearVelocity) * (2f * w)) * HeadGuideMass; - Head.ApplyCentralForce(force.LimitLength(HeadGuideForce)); - } - - private Vector3 _headGoalPrev, _headGoalVelocity; - private bool _haveHeadPrev; - private float _floorYForHead; - - private void ApplyStrength() - { - _appliedStrength = Strength; - float k = Mathf.Max(0.05f, Strength); - SetAngularCap(_spine.Joint, SpineTorque * k); - SetAngularCap(_neck.Joint, NeckTorque * k); - foreach (Arm a in _arms) - { - SetAngularCap(a.Shoulder.Joint, ShoulderTorque * k); - a.ShoulderCap = ShoulderTorque * k; - a.ElbowCap = ElbowTorque * k; - SetAngularCap(a.Guide.Joint, HandGuideTorque * k); - a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque * k); - } - } - - private static void SetAngularCap(Generic6DofJoint3D j, float cap) - { - j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - } - - private static void SetLinearCap(Generic6DofJoint3D j, float cap) - { - j.SetParamX(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); - j.SetParamY(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); - j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); - } - - // A hand pressing on or holding static world (a ledge, a wall, a grabbed bar) — not the floor: - // hands on the floor while crawling don't lift the body. - private static bool IsBraced(Arm a) - { - if (a.Held is StaticBody3D) - return true; - foreach (Node3D n in a.Hand.GetCollidingBodies()) - if (n is StaticBody3D && !n.IsInGroup(PropGrabber.NoGrabGroup)) - return true; - return false; - } - - // The lower-body collider hangs from the actual hips, its bottom never below a step above the - // feet; once it would have to slide far up into the torso (deep crouch, crawl) it stops colliding. - private void LegsDrive(TrueBodySolver s) - { - Transform3D pelvis = Pelvis.GlobalTransform; - Vector3 hip = pelvis * (-_pelvisCenterLocal); - float minCenter = _footY + MaxStepHeight + _legsHeight * 0.5f; - float center = Mathf.Max(hip.Y - _legsHeight * 0.5f, minCenter); - // Tuck (collider off, folded into the pelvis): deep crouch/crawl, or the hands bracing to - // lift the body (mantle, climbing — the knees come up). It STAYS tucked until it has been - // unneeded for a while AND its untucked spot is free: re-enabling inside a ledge face is - // what shoved the body off again and again (owner 2.2 "legs trying to get off the - // ground and instantly coming back"). - bool climbingHold = _arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D; - _lifted = Braced && (Pelvis.GlobalPosition.Y > _supportGoalY + MantleLift || climbingHold); - bool deep = center + _legsHeight * 0.5f >= hip.Y + LegsOffAbove; - bool wantTuck = deep || _lifted || (!_legsSolid && Braced); - _tuckFree = wantTuck ? 0f : _tuckFree + _dt; - Vector3 untucked = new(hip.X, center, hip.Z); - bool solid = _legsSolid ? !wantTuck : _tuckFree > UntuckDelay && LegsSpaceFree(untucked, s.YawBasis); - if (solid != _legsSolid) - { - _legsSolid = solid; - // The shape itself is disabled: Box3D keeps existing contacts when only the layer changes. - Legs.GetChild(0).SetDeferred(CollisionShape3D.PropertyName.Disabled, !solid); - } - if (!solid) - center = hip.Y; // tucked into the pelvis, out of the way - Vector3 desired = new(hip.X, center, hip.Z); - Quaternion frameA = Rot(Pelvis) * _legsDrive.FrameA; - SetLinearVelocity(_legsDrive.Joint, frameA.Inverse() * ((desired - Legs.GlobalPosition) * LegsKp)); - Quaternion target = (Rot(Pelvis) * _legsDrive.FrameA).Inverse() * (s.YawBasis.GetRotationQuaternion() * _legsDrive.FrameB); - Quaternion current = frameA.Inverse() * (Rot(Legs) * _legsDrive.FrameB); - SetAngularVelocity(_legsDrive.Joint, (BodyMath.RotationError(target, current) * LegsKp).LimitLength(MuscleMaxSpin)); - } - - // Shoulder = one drive: its linear motor keeps the humeral head on the chest's shoulder point - // shifted by the True Body girdle offset (shrug/protraction), its angular motor is the - // shoulder muscle (same PD + feed-forward as Muscle, parent = chest). - private void ShoulderDrive(Arm a, int side, float dt) - { - Drive d = a.Shoulder; - Transform3D chest = Chest.GlobalTransform.Orthonormalized(); - Vector3 desired = chest * (d.AnchorA + _girdle[side]); - Vector3 err = desired - a.Upper.GlobalTransform * d.AnchorB; - Quaternion frameA = chest.Basis.GetRotationQuaternion() * d.FrameA; - SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp)); - - Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB); - // Local muscle: relative to the chest's TARGET pose (never to the physical chest — that - // closes a rotation feedback loop through the arms). - Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse() - * (_targets[a.Upper].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameB); - Vector3 raw = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; - d.Ff = d.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); - d.PrevTarget = target; - d.HasPrev = true; - Vector3 spin = (d.Ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin); - SetAngularVelocity(d.Joint, spin); - // Which way is the muscle moving the arm? Rotating the bone direction downward = pulling. - Vector3 bone = (a.Upper.GlobalTransform.Basis.Y).Normalized(); - Vector3 boneMotion = (frameA * spin).Cross(bone); - float cap = (boneMotion.Y < 0f ? ShoulderPullTorque : ShoulderTorque) * Mathf.Max(0.05f, Strength); - if (Mathf.Abs(cap - a.ShoulderCap) > 0.5f) - { - a.ShoulderCap = cap; - SetAngularCap(d.Joint, cap); - } - } - - // Force-capped pull of the hand onto the controller, reacting into the chest (arm muscles). - private void HandGuide(Arm a, Transform3D target, Vector3 targetVelocity) - { - Transform3D chest = Chest.GlobalTransform; - Vector3 anchor = chest * a.Guide.AnchorA; - Vector3 anchorVelocity = Chest.LinearVelocity + Chest.AngularVelocity.Cross(anchor - Chest.GlobalPosition); - Vector3 err = target.Origin - a.Hand.GlobalPosition; - Vector3 desired = (targetVelocity + err * HandGuideKp).LimitLength(HandGuideMaxSpeed); - Quaternion frameA = Rot(Chest) * a.Guide.FrameA; - Vector3 shoulder = a.Upper.GlobalTransform * a.Shoulder.AnchorB; - Vector3 axis = a.Hand.GlobalPosition - shoulder; - // Axial share of the error, with a 3 cm floor so a nearly-reached target's noisy error - // direction can't flip a held load into "structural". - // PUSH only (the hand wants to go further from the shoulder — straight arms pressing on a - // ledge): bone in compression. Pulling (shortening the arm: pull-ups, climbing) is muscle - // work — the asymmetric shoulder/elbow muscles do it, and one arm can't lift the body. - float align = axis.LengthSquared() > 1e-8f ? Mathf.Max(0f, err.Dot(axis.Normalized())) / (err.Length() + StructuralDeadband) : 0f; - // Structure only helps against something: a hand in free air reaching past its arm's - // length must not haul the whole body after it. - if (a.Held == null && a.Hand.GetContactCount() == 0) - align = 0f; - float cap = Mathf.Lerp(HandGuideForce, HandStructuralForce, align) * Mathf.Max(0.05f, Strength); - if (Mathf.Abs(cap - a.GuideCap) > 1f) - { - a.GuideCap = cap; - SetLinearCap(a.Guide.Joint, cap); - } - SetLinearVelocity(a.Guide.Joint, frameA.Inverse() * (desired - anchorVelocity)); - // Angular: hand rotation onto the controller rotation, as a relative spin vs the chest. - Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand)); - Vector3 wWorld = (rotErr * HandGuideAngularKp).LimitLength(MuscleMaxSpin * 2f) - Chest.AngularVelocity; - SetAngularVelocity(a.Guide.Joint, frameA.Inverse() * wWorld); - } - - // "Leg strength": a stiff, mass-scaled support holding the pelvis at its pose height above the - // ground and carrying it after the head horizontally (friction-limited), plus an upright/yaw - // balance motor (world anchor → pelvis). No ground → no support (falls); balance weakens in the air. - private void GroundControl(Transform3D target, float floorY, float dt) - { - Vector3 pos = Pelvis.GlobalPosition; - float hipAboveFloor = target.Origin.Y - floorY; - var query = PhysicsRayQueryParameters3D.Create(pos, pos + Vector3.Down * (hipAboveFloor + GroundProbeExtra + 0.5f), WorldMask); - Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query); - bool wasGrounded = Grounded; - Grounded = false; - _groundBody = null; - if (hit.Count > 0) - { - Vector3 point = (Vector3)hit["position"]; - Vector3 normal = (Vector3)hit["normal"]; - _groundBody = GodotObject.InstanceFromId((ulong)hit["collider_id"]) as RigidBody3D; // Box3D: "collider" is null - _groundPoint = point; - bool inReach = pos.Y - point.Y < hipAboveFloor + GroundProbeExtra; - bool steep = normal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees)); - if (inReach && steep && wasGrounded && point.Y <= _footY + MaxStepHeight) - { - // The foot of a too-steep slope: keep standing on the ground we're on (the legs - // collider stops us); dropping support here would collapse the body. - Grounded = true; - } - else if (inReach && !steep) - { - // Step rule: ground more than a step above the feet is only stood on once the - // hips are already well above it (climbed/mantled/landed there) — otherwise it's an - // obstacle the legs collider holds us back from, and the feet stay where they were. - // Higher ground climbed onto must be under the hips' back edge too (not just a - // pelvis centre hanging over the lip). - bool clear = point.Y <= _footY + MaxStepHeight || GroundBehindAt(pos, point.Y); - bool reachable = point.Y <= _footY + MaxStepHeight - || ((!wasGrounded || _lifted || pos.Y - point.Y > 0.6f * hipAboveFloor) && clear); - Grounded = true; - if (reachable) - _footY = point.Y; - } - } - - // Feed-forward from the GOAL (pose + ground offset): when the play-space floor follows the - // ground, the pose target and the ground offset move by the same amount in opposite - // directions — differentiating the pose alone would launch the body. - Vector3 goalNow = target.Origin + Vector3.Up * (_footY - floorY); - Vector3 rawVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero; - _pelvisTargetPrev = goalNow; - _havePelvisPrev = true; - if (rawVelocity.Length() > 10f) - rawVelocity = Vector3.Zero; - _goalVelocity = _goalVelocity.Lerp(rawVelocity, LowPass(dt, GroundFeedForwardTau)); - // Held back = the hips can't get to where the pose wants them (knee-high box, table edge, - // wall). Measured at the hips, not the head: the body tips over an obstacle, so the head - // may be right where the camera is while the hips are stuck. - Vector3 hipLag = new(target.Origin.X - pos.X, 0f, target.Origin.Z - pos.Z); - HeldBack = Grounded && hipLag.Length() > FreeSlack ? hipLag.Normalized() : Vector3.Zero; - Vector3 targetVelocity = _goalVelocity; - - float weight = BodyMass * 9.8f; - if (Grounded) - { - // Pose heights are relative to the tracking floor; stand on the ground actually under us. - Vector3 goal = target.Origin + Vector3.Up * (GroundY - floorY); - _supportGoalY = goal.Y; - float w = Mathf.Tau * GroundFrequency; - Vector3 accel = (goal - pos) * (w * w) + (targetVelocity - Pelvis.LinearVelocity) * (2f * GroundDamping * w); - Vector3 force = accel * BodyMass; - // Vertical as a rate-limited velocity servo: full leg strength brakes a landing, but - // the legs raise the body at most MaxRiseSpeed — they can push, not pull, so a hard push - // toward a higher stance (onto a ledge) would otherwise overshoot into a jump. - float riseVelocity = Mathf.Clamp((goal.Y - pos.Y) * w, -3f, MaxRiseSpeed) + targetVelocity.Y; - float verticalAccel = (riseVelocity - Pelvis.LinearVelocity.Y) * (2f * GroundDamping * w); - float support = Mathf.Clamp(verticalAccel * BodyMass + weight, 0f, LegStrength * weight); - float carry = Mathf.Max(GroundFriction * support, _legsSolid ? 0f : CrawlCarry); - // Hips held back by an obstacle: lean on it, don't bulldoze — full leg force tipped the - // body over knee-high boxes and steep slopes (and the riding view tipped with it). - if (HeldBack != Vector3.Zero) - carry = Mathf.Min(carry, BlockedPush); - Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(carry); - Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support); - // Newton's third law: standing on something movable (seesaw, plank, crate) pushes it - // down — the feet aren't physical, so the reaction is applied where the ground ray hit. - if (_groundBody != null && IsInstanceValid(_groundBody)) - _groundBody.ApplyForce(-(horizontal + Vector3.Up * support), _groundPoint - _groundBody.GlobalPosition); - } - else if (Braced) - { - // Hanging/mantling on the hands: the hips still drift after the head a little, so you can - // shift your weight over a ledge you're pressing yourself up onto. - Vector3 lead = target.Origin - pos; - lead.Y = 0f; - Vector3 v = Pelvis.LinearVelocity; - v.Y = 0f; - Pelvis.ApplyCentralForce(((lead * 20f - v * 8f) * BodyMass).LimitLength(BracedAirControl)); - } - - Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(Pelvis)); - Vector3 spin = (rotErr * BalanceKp).LimitLength(MuscleMaxSpin); - SetAngularVelocity(_balance.Joint, _balance.FrameA.Inverse() * spin); // anchor = world frame - float cap = BalanceTorque * (Grounded ? 1f : AirBalanceShare); - if (cap != _balanceCap) - { - _balanceCap = cap; - _balance.Joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - _balance.Joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - _balance.Joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); - } - } - - private Drive _balance; - private float _balanceCap; - - // ---------------------------------------------------------------- play space - - // How the driver should move its play-space origin after the last physics step. ONE rule, always, - // in all directions (Boneworks/BIMOS: the rig rides the body): the camera rides the character's - // eye. Head motion leads the body by at most ViewDeadzone (eased) / MaxViewLead (hard) — less - // than the head box reaches in front of the eye, so the view stays inside the head. A sphere - // swept eye→camera snaps the view back at once when geometry is in the way. - // This does NOT feed back into the pose: the True Body reads the head relative to the play-space - // floor (crouch = local head height) and ground control re-bases its goal onto the real ground - // (footY − floorY), so moving the play space vertically changes neither. (2.3 split height off - // to a separate floor-follow out of fear of that loop; the two then fought over the same height, - // and landings left the view hanging above a sinking body — owner 2.4.) - public Vector3 OriginCorrection(Vector3 camera, float floorY, float dt) - { - if (!Built) - return Vector3.Zero; - Vector3 eye = Eye; - Vector3 lead = camera - eye; - Vector3 correction = Vector3.Zero; - if (lead.LengthSquared() > 1e-8f) - { - _cameraQuery ??= new PhysicsShapeQueryParameters3D { Shape = new SphereShape3D { Radius = CameraRadius }, CollisionMask = WorldMask }; - _cameraQuery.Transform = new Transform3D(Basis.Identity, eye); - _cameraQuery.Motion = lead; - float[] fractions = GetWorld3D().DirectSpaceState.CastMotion(_cameraQuery); - float safe = fractions.Length > 0 ? fractions[0] : 1f; - if (safe < 1f) - correction = -lead * (1f - safe); - Vector3 rest = lead + correction; - if (rest.Length() > ViewDeadzone) - correction -= rest * (1f - ViewDeadzone / rest.Length()) * (1f - Mathf.Exp(-dt / ViewFollowTau)); - rest = lead + correction; - if (rest.Length() > MaxViewLead) - correction -= rest * (1f - MaxViewLead / rest.Length()); - } - // Slow re-centering of the play-space floor onto the real ground while standing: removes the - // small height offsets the riding view picks up (a transient lag, a landing). The pose and the - // head guide are re-based on the real ground, so this only brings the camera onto the eye — - // the same direction as the follow above, never against it. - if (Grounded) - correction.Y += (_footY - floorY) * (1f - Mathf.Exp(-dt / FloorRecenterTau)); - // The pose target moves with the play space: a correction is not intended motion, so it - // must not show up in ground control's feed-forward (that loop ran away at 7 m/s). - _pelvisTargetPrev += correction; - _headGoalPrev += correction; - return correction; - } - - private PhysicsShapeQueryParameters3D _cameraQuery; - - // Second ground ray 12 cm behind the hips (toward the body's back): ground at least `groundY`. - private bool GroundBehindAt(Vector3 pelvisPos, float groundY) - { - Vector3 back = Pelvis.GlobalTransform.Basis.Z; - back.Y = 0f; - back = back.LengthSquared() > 1e-6f ? back.Normalized() * 0.12f : Vector3.Zero; - var query = PhysicsRayQueryParameters3D.Create(pelvisPos + back, pelvisPos + back + Vector3.Down * 3f, WorldMask); - Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query); - return hit.Count > 0 && ((Vector3)hit["position"]).Y > groundY - 0.05f; - } - - public void TeleportBy(Transform3D delta) - { - if (!Built) - return; - foreach (RigidBody3D b in _bodies) - { - b.GlobalTransform = delta * b.GlobalTransform; - b.LinearVelocity = delta.Basis * b.LinearVelocity; - b.AngularVelocity = delta.Basis * b.AngularVelocity; - } - _pelvisTargetPrev = delta * _pelvisTargetPrev; - _footY += delta.Origin.Y; - foreach (RigidBody3D b in _bodies) - b.ResetPhysicsInterpolation(); - } - - public void SnapToPose(TrueBodySolver s, Transform3D left, Transform3D right) - { - ComputeTargets(s, left, right); - foreach (RigidBody3D b in _bodies) - { - b.GlobalTransform = Target(b); - b.LinearVelocity = Vector3.Zero; - b.AngularVelocity = Vector3.Zero; - } - // The hands sit at the wrist end of the solved arm, not at a possibly unreachable controller. - for (int side = 0; side < 2; side++) - { - Arm a = _arms[side]; - Basis hb = _targets[a.Hand].Basis; - a.Hand.GlobalTransform = new Transform3D(hb, s.Wrist[side] - hb * new Vector3(0f, 0f, WristOffset)); - } - _havePelvisPrev = false; - foreach (RigidBody3D b in _bodies) - b.ResetPhysicsInterpolation(); - } - - // Collision exceptions between a held prop and that arm's segments (its hand is excluded by - // the grab weld itself) — a forearm wrapped around a held object must not fight it. - public void SetHeld(int side, PhysicsBody3D prop) - { - if (!Built) - return; - Arm a = _arms[side]; - if (a.Held != null && IsInstanceValid(a.Held) && a.Held != _arms[1 - side].Held) - { - a.Upper.RemoveCollisionExceptionWith(a.Held); - a.Fore.RemoveCollisionExceptionWith(a.Held); - } - a.Held = prop; - if (prop != null) - { - a.Upper.AddCollisionExceptionWith(prop); - a.Fore.AddCollisionExceptionWith(prop); - } - } + public const float BodyMass = 60f; // Nimso's character is 60 kg + // Muscles: PD gain (1/s), max commanded spin (rad/s), torque cap (N·m). + public const float SpineKp = 15f, SpineTorque = 400f; + public const float NeckKp = 20f, NeckTorque = 60f; + // Asymmetric by direction (Nimso; human strength tables): pulling the arm DOWN (lats/back — + // pull-ups, hanging, climbing) is roughly twice as strong as raising it (front delts — a weight + // held out front), and elbow flexion (biceps) beats extension. + public const float ShoulderKp = 20f, ShoulderTorque = 35f, ShoulderPullTorque = 100f; + public const float ElbowKp = 20f, ElbowTorque = 45f, ElbowFlexTorque = 70f; + public const float MuscleMaxSpin = 15f; + // Feed-forwards are derivatives of tracking: low-pass them or tracking jitter (±1–2 mm per + // sample) becomes hundreds of newtons of buzz (RagdollSpike REAL input mode). + public const float MuscleFeedForwardTau = 0.05f; // s + public const float GroundFeedForwardTau = 0.12f; // s + public const float GirdleKp = 30f, GirdleForce = 2000f; // shoulder drive, linear part + // Hand guide (chest→hand drive): pulls the hand onto the controller; capped so the arm, + // not the guide, carries real loads. + public const float HandGuideKp = 40f, HandGuideForce = 40f, HandGuideMaxSpeed = 10f; + // STRUCTURAL strength: force along the arm (straight arm pressing on a ledge, hanging from a + // bar, carrying a bucket at your side) goes through bone and is strong; force across the arm + // needs muscle torque and stays at HandGuideForce. Cap = lerp(base, structural, alignment). + // Two arms ≈ 1.2 × body weight → two hands mantle/pull up, one can't (Nimso). + public const float HandStructuralForce = 400f; + public const float StructuralDeadband = 0.03f; + // Nimso's one global strength value: scales every muscle and the hand guides (menu slider). + public static float Strength = 1f; + public const float HandGuideAngularKp = 40f, HandGuideTorque = 25f; + // Ground control (world-space support on the pelvis, only while grounded). + public const float GroundFrequency = 4f; // Hz + public const float GroundDamping = 1f; + public const float LegStrength = 4.5f; // max support, × body weight (real legs brake a landing at 3–5 g; 2× sank ~0.4 m) + public const float GroundFriction = 1f; // horizontal force ≤ this × support + public const float GroundProbeExtra = 0.3f; // ground counts while within hip height + this + public const float MaxStepHeight = 0.35f; + public const float MaxRiseSpeed = 1.2f; // m/s the legs lift the body toward its stance + // Landing (owner 2.6: the sink is feedback, it just had to be followed by the view): falling legs + // only touch down when the hips are within their real reach of the ground. Before, support + // began GroundProbeExtra early and braked the fall in the air — no dip at all. Now LegStrength + // stops the body BELOW its stance, so the dip scales with the impact: ~0.13 m from a hop, + // ~0.3 m from 1 m, legs fully folded from a big fall (Boneworks) — then the normal rise. + public const float LandingReach = 0.05f; // m past the pose's hip height + public const float AbsorbTime = 0.4f; // s after a hard touchdown the head guide stays off (it would hold the dip up) + // Jump (right A): a short countermovement dip, then the legs push JumpForce over the stroke up to + // full extension. Takeoff speed is physics — v² = 2·(F/m − g)·stroke — so a crouch before + // the press (real or stick) jumps higher: ~0.45 m standing, ~0.7 m from a deep crouch. + public const float JumpForce = 2.4f; // × body weight + public const float JumpDip = 0.2f; // m the hips drop first when pressed from standing + public const float JumpExtension = 0.06f; // m above the standing hips at toe-off + public const float JumpMaxStroke = 0.5f; + public const float JumpBuffer = 0.15f; // s a press is remembered before touchdown + public const float CoyoteTime = 0.12f; // s after leaving the ground a jump still works + public const float MaxSlopeDegrees = 35f; // steeper ground gives no support (Nimso: "slopes beyond an angle can't be run up") // higher ground under the hips isn't stood on unless the body got up there + public const float BracedAirControl = 150f; // N of horizontal hip carry while airborne but braced on the hands (mantling) + public const float CrawlCarry = 300f; + public const float BlockedPush = 150f; // N horizontal leg push while the hips are held back // N: the torso lies on the floor in a crawl; knees/hands would push (not simulated) + public const float TorsoFriction = 0.3f; // clothing on floor: lets a crawl slide + public const float LimbFriction = 0.25f; + // Lower-body collider: a box from the hips down to MaxStepHeight above the feet, so knee-high + // obstacles block walking (no walking over tables) while lower steps pass under it. It slides + // up into the torso when crouching and switches off when crawling (Nimso: legs don't snag). + public const float LegsMass = 6f; + public const float LegsKp = 30f, LegsForce = 3000f, LegsTorque = 400f, LegsMaxSpeed = 1.5f; + public const float LegsOffAbove = 0.25f; // collider disabled once slid this far above the hips + public const float MantleLift = 0.03f; // braced + hips this far above the support goal = being lifted (legs tuck) + public const float UntuckDelay = 0.3f; // s the tuck must be unneeded before the collider returns + // Balance: an implicit torque-capped motor from a world anchor to the pelvis (explicit torques + // let the arms' reaction torques set the whole trunk swinging). + public const float BalanceKp = 10f; + public const float BalanceTorque = 800f; + public const float AirBalanceShare = 0.3f; + // Play-space reconciliation. + public const float CameraRadius = 0.03f; // sphere swept from the head's eye to the camera + public const float FreeSlack = 0.12f; // camera leading the body's eye by more than this = body held back: stick stops pushing that way + public const float ViewDeadzone = 0.02f; // m the head may lead the body before the view is eased back onto it + public const float ViewFollowTau = 0.04f; // s + public const float MaxViewLead = 0.04f; // hard: the head box reaches 2 cm in front of the eye + public const float FloorRecenterTau = 1f; // s + public const float HeadGuideFrequency = 5f; // Hz + public const float HeadGuideMass = 15f; // kg effective (head + part of the trunk) + public const float HeadGuideForce = 250f; // N + public const float ResetDistance = 1.5f; // pelvis this far from its target → snap the whole body to the pose + // Geometry. + public const float LumbarFraction = 0.35f; // lumbar joint height above the hips, × trunk + public const float PelvisLeanShare = 0.5f; // share of the crouch lean taken by the hips + public static readonly Vector3 HeadCenterFromEye = new(0f, -0.005f, 0.08f); // crown ~10 cm above the eyes, chin ~11 cm below (was 12.5 / 8.5) + public static readonly Vector3 HeadSize = new(0.16f, 0.21f, 0.2f); + public static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f); + public const float WristOffset = TrueBodySolver.WristOffset; + public const float ElbowMaxFlex = 2.6f; // rad (~150°) + // Sign of HingeJoint3D's angle/motor for a positive rotation of the child about the joint's + // +Z (measured by RagdollSpike's "hinge sign" phase). + public static float HingeSign = 1f; + + // Collision layers (AGENTS.md): 3 body, 4 hands, 5 arms. + private const uint BodyLayer = 0b100, HandLayer = 0b1000, ArmLayer = 0b10000; + private const uint WorldMask = 0b1; + + private sealed class Ball + { + public Generic6DofJoint3D Joint; + public RigidBody3D Parent, Child; + public Quaternion FrameA, FrameB; // joint frame relative to each body + public float Kp; + public Quaternion PrevTarget; + public bool HasPrev; + public Vector3 Ff; + } + + private sealed class Drive + { + public Generic6DofJoint3D Joint; + public Quaternion FrameA, FrameB; + public Vector3 AnchorA, AnchorB; // anchor point, parent-local / child-local + public Quaternion PrevTarget; + public bool HasPrev; + public Vector3 Ff; + } + + private sealed class Arm + { + public RigidBody3D Upper, Fore, Hand; + public Ball Wrist; + public HingeJoint3D Elbow; + public Drive Shoulder, Guide; + public float UpperLen, ForeLen; + public float PrevFlexTarget = -1f; + public float FlexFf; + public float GuideCap = HandGuideForce; + public float ShoulderCap = ShoulderTorque, ElbowCap = ElbowTorque; + public PhysicsBody3D Held; + } + + public RigidBody3D Pelvis { get; private set; } + public RigidBody3D Chest { get; private set; } + public RigidBody3D Head { get; private set; } + public bool Built { get; private set; } + public bool Grounded { get; private set; } + public float GroundY => _footY; + public bool Braced { get; private set; } // a hand touches or holds static world + public bool LegsSolid => _legsSolid; + public bool Lifted => _lifted; + // Horizontal direction in which the body is held back from the camera (zero if not): the stick + // must not keep moving the play space that way. + public Vector3 HeldBack { get; private set; } + public bool Jumping => _jump != JumpPhase.None; + // Really climbing: a hand holds static world, or the arms are lifting the body (mantle). + public bool Climbing => Built && (_arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D || _lifted); + public RigidBody3D Legs { get; private set; } + public Vector3 Eye => Head.GlobalTransform * _eyeLocal; + public BodyProfile Profile { get; private set; } + public int Snaps { get; private set; } // safety resets (NaN / too far) — should stay 0 + + private readonly Arm[] _arms = { new(), new() }; + private readonly List _bodies = new(); + private Ball _spine, _neck; + private Vector3 _neckBaseToEye; // head frame, canonical (neck straight) + private float _trunk, _lumbar, _neckLen; + private Vector3 _eyeLocal; + private Vector3 _pelvisCenterLocal, _chestCenterLocal, _headCenterLocal; // from each body's proximal joint, in its basis + private readonly Dictionary _targets = new(); + private readonly Vector3[] _girdle = new Vector3[2]; // True Body shoulder offset, chest frame + private float _footY; + private float _supportGoalY; + private RigidBody3D _groundBody; + private Vector3 _groundPoint; + private float _tuckFree, _dt; + private PhysicsShapeQueryParameters3D _legsQuery; + + private bool LegsSpaceFree(Vector3 center, Basis basis) + { + _legsQuery ??= new PhysicsShapeQueryParameters3D { Shape = Legs.GetChild(0).Shape, CollisionMask = WorldMask }; + _legsQuery.Transform = new Transform3D(basis, center); + return GetWorld3D().DirectSpaceState.IntersectShape(_legsQuery, 1).Count == 0; + } + private Vector3 _goalVelocity; + private enum JumpPhase { None, Dip, Push, Flight } + private JumpPhase _jump; + private float _jumpT, _jumpRequest, _airT, _jumpFromY, _sinceLanding = float.MaxValue; + + // Right A. Buffered, so a press just before touchdown still jumps. + public void Jump() => _jumpRequest = JumpBuffer; + + private float _standingHip; // hips of the standing pose above the feet = the legs' full extension + + private static float LowPass(float dt, float tau) => 1f - Mathf.Exp(-dt / tau); + private bool _lifted; + private float _legsHeight; + private Drive _legsDrive; + private bool _legsSolid = true; + private ProceduralLegs _legVisuals; + private float _appliedStrength = 1f; + private Vector3 _pelvisTargetPrev; + private bool _havePelvisPrev; + + public RigidBody3D Hand(int side) => Built ? _arms[side].Hand : null; + public RigidBody3D UpperArm(int side) => Built ? _arms[side].Upper : null; + public RigidBody3D Forearm(int side) => Built ? _arms[side].Fore : null; + public IReadOnlyList Bodies => _bodies; + + // ---------------------------------------------------------------- build + + public void Build(BodyProfile p, TrueBodySolver pose, Transform3D left, Transform3D right) + { + Clear(); + Profile = p; + _trunk = TrueBodySolver.TrunkRatio * p.Stature; + _lumbar = LumbarFraction * _trunk; + _neckLen = Mathf.Max(0.05f, p.ShoulderDrop + TrueBodySolver.EyeToNeckPivot.Y - TrueBodySolver.NeckBaseAboveShoulders); + float m = BodyMass; + float s = p.Stature / 1.75f; + + // Canonical pose (standing, level head, arms straight down-forward-out) in frame C at the + // current pelvis: joint cones/twists are centred on it and the elbow hinge's zero is a + // straight arm. The body is then moved onto the live pose. + Transform3D c = new(pose.YawBasis, pose.PelvisTarget); + Vector3 hip = Vector3.Zero; + Vector3 lumbar = new(0f, _lumbar, 0f); + Vector3 upperChest = new(0f, _trunk, 0f); + Vector3 neckBase = upperChest + new Vector3(0f, TrueBodySolver.NeckBaseAboveShoulders, + -(TrueBodySolver.ShouldersBehindEyes - TrueBodySolver.EyeToNeckPivot.Z)); + Vector3 neckPivot = neckBase + new Vector3(0f, _neckLen, 0f); + _eyeLocal = -TrueBodySolver.EyeToNeckPivot; // head body origin is set to its centre below + Vector3 headCenterFromPivot = _eyeLocal + HeadCenterFromEye; + + Vector3 pelvisSize = new(0.17f * p.Stature, _lumbar + 0.06f, 0.18f * s); + _pelvisCenterLocal = new Vector3(0f, _lumbar * 0.5f - 0.03f, 0f); + float chestHeight = _trunk - _lumbar + TrueBodySolver.NeckBaseAboveShoulders; + Vector3 chestSize = new(2f * p.ShoulderHalfWidth * 0.75f, chestHeight, 0.2f * s); + _chestCenterLocal = new Vector3(0f, chestHeight * 0.5f, 0f); + _headCenterLocal = headCenterFromPivot; + _eyeLocal -= headCenterFromPivot; // eye relative to the head body's centre + + Color skin = new(0.85f, 0.72f, 0.6f), shirt = new(0.3f, 0.45f, 0.7f), pants = new(0.25f, 0.27f, 0.32f); + Pelvis = MakeBody("Pelvis", new BoxShape3D { Size = pelvisSize }, (0.142f + 0.139f + 0.322f) * m - LegsMass, BodyLayer, WorldMask, pants, + c * new Transform3D(Basis.Identity, hip + _pelvisCenterLocal), friction: TorsoFriction); + float standingHip = pose.PelvisTarget.Y - (pose.Eye.Y - p.EyeHeight); + _standingHip = standingHip + _pelvisCenterLocal.Y; // of the pelvis BODY (what ground control holds) + _legsHeight = Mathf.Max(0.2f, standingHip - MaxStepHeight); + Legs = MakeBody("Legs", new BoxShape3D { Size = new Vector3(0.28f * s, _legsHeight, 0.2f * s) }, LegsMass, BodyLayer, WorldMask, pants, + c * new Transform3D(Basis.Identity, hip - new Vector3(0f, _legsHeight * 0.5f, 0f)), visible: false); + // Explicit inertia: the collider is switched off when tucked, and a body without an + // enabled shape has no inertia — its 400 N·m drive then tumbled the whole pelvis + // (owner 2.1 "body spinning" when crouched). + Legs.Inertia = new Vector3(_legsHeight * _legsHeight + 0.04f, 0.28f * 0.28f + 0.04f, _legsHeight * _legsHeight + 0.08f) * (LegsMass / 12f); + _footY = pose.Eye.Y - p.EyeHeight; + Chest = MakeBody("Chest", new BoxShape3D { Size = chestSize }, 0.216f * m, BodyLayer, WorldMask, shirt, + c * new Transform3D(Basis.Identity, lumbar + _chestCenterLocal), friction: TorsoFriction); + Head = MakeBody("Head", new BoxShape3D { Size = HeadSize * s }, 0.081f * m, BodyLayer, WorldMask, skin, + c * new Transform3D(Basis.Identity, neckPivot + headCenterFromPivot), friction: TorsoFriction); + // The neck: a box hanging under the head, same body. + Vector3 neckCenter = (neckBase + neckPivot) * 0.5f - (neckPivot + headCenterFromPivot); + Head.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter }); + MeshInstance3D neckMesh = new() { Mesh = new BoxMesh { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter }; + neckMesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = skin, Roughness = 0.6f }); + Head.AddChild(neckMesh); + _neckBaseToEye = neckPivot - TrueBodySolver.EyeToNeckPivot - neckBase; + + // World anchor for the balance motor (no shape, linear motor force 0 = position free). + StaticBody3D anchor = new() { Name = "BalanceAnchor", CollisionLayer = 0, CollisionMask = 0 }; + AddChild(anchor); + _balance = MakeDrive(anchor, Pelvis, Pelvis.GlobalTransform, 0f, BalanceTorque); + _legsDrive = MakeDrive(Pelvis, Legs, c * new Transform3D(Basis.Identity, hip), LegsForce, LegsTorque); + _legVisuals = new ProceduralLegs { Name = "LegVisuals" }; + AddChild(_legVisuals); + _legVisuals.Setup(Pelvis, -_pelvisCenterLocal, p.Stature, pants, new Color(0.15f, 0.15f, 0.17f)); + _balanceCap = BalanceTorque; + + _spine = MakeBall(Pelvis, Chest, c * lumbar, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(60f), Mathf.DegToRad(35f), SpineKp, SpineTorque); + _neck = MakeBall(Chest, Head, c * neckBase, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(75f), Mathf.DegToRad(85f), NeckKp, NeckTorque); + + for (int side = 0; side < 2; side++) + { + Arm a = _arms[side]; + float sign = side == 0 ? -1f : 1f; + a.UpperLen = p.UpperArm; + a.ForeLen = p.Forearm; + Vector3 shoulder = upperChest + new Vector3(sign * p.ShoulderHalfWidth, 0f, 0f); + Vector3 dir = new Vector3(sign * 0.42f, -0.74f, -0.47f).Normalized(); + Vector3 reference = Vector3.Down * 2f + Vector3.Back * 0.8f + new Vector3(sign * 0.3f, 0f, 0f); + Vector3 pole = (reference - dir * reference.Dot(dir)).Normalized(); + Vector3 hinge = pole.Cross(dir).Normalized(); + Basis bone = BoneBasis(dir, hinge); + Vector3 elbow = shoulder + dir * a.UpperLen; + Vector3 wrist = elbow + dir * a.ForeLen; + Vector3 handX = (Vector3.Right - dir * dir.X).Normalized(); + Basis handBasis = new(handX, (-dir).Cross(handX), -dir); + Vector3 handOrigin = wrist + dir * WristOffset; + string n = side == 0 ? "Left" : "Right"; + + a.Upper = MakeBody(n + "UpperArm", new BoxShape3D { Size = new Vector3(0.085f * s, a.UpperLen, 0.085f * s) }, 0.028f * m, ArmLayer, WorldMask, shirt, + c * new Transform3D(bone, shoulder + dir * (a.UpperLen * 0.5f)), friction: LimbFriction); + a.Fore = MakeBody(n + "Forearm", new BoxShape3D { Size = new Vector3(0.07f * s, a.ForeLen - 0.01f, 0.07f * s) }, 0.016f * m, ArmLayer, WorldMask, skin, + c * new Transform3D(bone, elbow + dir * (a.ForeLen * 0.5f)), friction: LimbFriction); + a.Hand = MakeBody(n + "Hand", new BoxShape3D { Size = HandSize }, 0.006f * m, HandLayer, WorldMask | HandLayer, + side == 0 ? new Color(0.9f, 0.3f, 0.35f) : new Color(0.3f, 0.5f, 0.9f), c * new Transform3D(handBasis, handOrigin), friction: LimbFriction); + a.Hand.ContinuousCd = true; + a.Hand.ContactMonitor = true; + a.Hand.MaxContactsReported = 8; + + a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(Basis.Identity, shoulder), GirdleForce, ShoulderTorque); + a.Elbow = MakeHinge(a.Upper, a.Fore, c * elbow, c.Basis * new Basis(dir, hinge.Cross(dir), hinge)); + a.Wrist = MakeBall(a.Fore, a.Hand, c * wrist, c.Basis * TwistAlong(dir), Mathf.DegToRad(80f), Mathf.DegToRad(90f), 0f, 0f); + a.Guide = MakeDrive(Chest, a.Hand, c * new Transform3D(handBasis, handOrigin), HandGuideForce, HandGuideTorque); + } + + Built = true; + _havePelvisPrev = false; + _appliedStrength = 1f; + _legsSolid = true; + ApplyStrength(); + SnapToPose(pose, left, right); + GD.Print($"PlayerRagdoll: built for stature {p.Stature:F2} m (arm {p.UpperArm:F3}+{p.Forearm:F3} m, {_bodies.Count} bodies)"); + } + + public void Clear() + { + foreach (Node child in GetChildren()) + child.QueueFree(); + foreach (Node child in GetChildren()) + RemoveChild(child); + _bodies.Clear(); + _targets.Clear(); + for (int i = 0; i < 2; i++) + _arms[i] = new Arm(); + Built = false; + _jump = JumpPhase.None; + _jumpRequest = 0f; + _sinceLanding = float.MaxValue; + } + + // Twist axis (joint X) along `dir`. + private static Basis TwistAlong(Vector3 dir) + { + Vector3 helper = Mathf.Abs(dir.Y) < 0.9f ? Vector3.Up : Vector3.Right; + Vector3 z = dir.Cross(helper).Normalized(); + return new Basis(dir, z.Cross(dir), z); + } + + // Segment basis: Y along the bone, X = elbow hinge axis. + private static Basis BoneBasis(Vector3 boneDir, Vector3 hinge) + { + Vector3 x = (hinge - boneDir * hinge.Dot(boneDir)).Normalized(); + return new Basis(x, boneDir, x.Cross(boneDir)); + } + + // NOTE (Box3D wrapper): friction is read only when the body's shapes are created — the material + // must be set before AddChild (setting it later silently does nothing). + private RigidBody3D MakeBody(string name, Shape3D shape, float mass, uint layer, uint mask, Color color, Transform3D at, bool visible = true, float friction = 1f) + { + RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at, ContactMonitor = true, MaxContactsReported = 4, + PhysicsMaterialOverride = new PhysicsMaterial { Friction = friction } }; + body.AddChild(new CollisionShape3D { Shape = shape }); + if (shape is BoxShape3D box && layer != 0 && visible) + { + MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = box.Size } }; + mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f }); + body.AddChild(mesh); + } + AddChild(body); + _bodies.Add(body); + return body; + } + + private static Quaternion Rot(Node3D n) => n.GlobalTransform.Basis.Orthonormalized().GetRotationQuaternion(); + + // Configure BEFORE AddChild (the node pushes everything to the server on enter-tree). + private Ball MakeBall(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame, float cone, float twist, float kp, float torque) + { + Generic6DofJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; + j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -twist); + j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, twist); + foreach (var set in new System.Action[] { j.SetParamY, j.SetParamZ }) + { + set(Generic6DofJoint3D.Param.AngularLowerLimit, -cone); + set(Generic6DofJoint3D.Param.AngularUpperLimit, cone); + } + if (torque > 0f) + { + j.SetFlagX(Generic6DofJoint3D.Flag.EnableMotor, true); + j.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, true); + j.SetFlagZ(Generic6DofJoint3D.Flag.EnableMotor, true); + j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); + j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); + j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); + } + AddChild(j); + Quaternion jq = frame.Orthonormalized().GetRotationQuaternion(); + return new Ball { Joint = j, Parent = parent, Child = child, FrameA = Rot(parent).Inverse() * jq, FrameB = Rot(child).Inverse() * jq, Kp = kp }; + } + + private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque) + { + Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; + foreach (var flag in new System.Action[] { j.SetFlagX, j.SetFlagY, j.SetFlagZ }) + { + flag(Generic6DofJoint3D.Flag.EnableLinearLimit, false); + flag(Generic6DofJoint3D.Flag.EnableAngularLimit, false); + flag(Generic6DofJoint3D.Flag.EnableLinearMotor, true); + flag(Generic6DofJoint3D.Flag.EnableMotor, true); + } + foreach (var param in new System.Action[] { j.SetParamX, j.SetParamY, j.SetParamZ }) + { + param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force); + param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); + } + AddChild(j); + Quaternion jq = at.Basis.Orthonormalized().GetRotationQuaternion(); + return new Drive + { + Joint = j, + FrameA = Rot(parent).Inverse() * jq, + FrameB = Rot(child).Inverse() * jq, + AnchorA = parent.GlobalTransform.AffineInverse() * at.Origin, + AnchorB = child.GlobalTransform.AffineInverse() * at.Origin, + }; + } + + private HingeJoint3D MakeHinge(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame) + { + HingeJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; + float lo = HingeSign > 0f ? 0f : -ElbowMaxFlex; + float hi = HingeSign > 0f ? ElbowMaxFlex : 0f; + j.SetFlag(HingeJoint3D.Flag.UseLimit, true); + j.SetParam(HingeJoint3D.Param.LimitLower, lo); + j.SetParam(HingeJoint3D.Param.LimitUpper, hi); + j.SetFlag(HingeJoint3D.Flag.EnableMotor, true); + j.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque); + AddChild(j); + return j; + } + + // ---------------------------------------------------------------- pose targets + + // World transform every body should have in the True Body pose. The trunk is rebuilt top-down + // through the physical chain (upper chest → lumbar → hips) so a leaning chest doesn't drag + // the head off the camera; the hips take part of the crouch lean (hip hinge). + private void ComputeTargets(TrueBodySolver s, Transform3D left, Transform3D right, bool physicalArms = false, float dt = 0f) + { + Basis chest = s.ChestBasis; + // Hips take half of a crouch lean, all of it in a crawl (keeps them off the floor). + float share = Mathf.Lerp(PelvisLeanShare, 1f, Mathf.Clamp((s.Lean - TrueBodySolver.ChestLeanMax) / 0.5f, 0f, 1f)); + Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * share); + Vector3 lumbar = s.UpperChest - chest.Y * (_trunk - _lumbar); + Vector3 hip = lumbar - pelvis.Y * _lumbar; + _targets[Pelvis] = new Transform3D(pelvis, hip + pelvis * _pelvisCenterLocal); + _targets[Chest] = new Transform3D(chest, lumbar + chest * _chestCenterLocal); + _targets[Legs] = new Transform3D(s.YawBasis, hip - Vector3.Up * (_legsHeight * 0.5f)); // snap pose only; LegsDrive steps it + // Head + neck pivot at the neck base: orient like the camera, tilted by the small extra + // rotation that puts the body's eye on the camera (the True Body bends the neck at two points). + Vector3 eyeDir = s.HeadBasis * _neckBaseToEye; + Vector3 wantDir = s.Eye - s.NeckBase; + Quaternion tilt = eyeDir.LengthSquared() > 1e-8f && wantDir.LengthSquared() > 1e-8f + ? new Quaternion(eyeDir.Normalized(), wantDir.Normalized()) : Quaternion.Identity; + Basis head = new Basis(tilt) * s.HeadBasis; + _targets[Head] = new Transform3D(head, s.NeckBase + head * (_neckBaseToEye - _eyeLocal)); + + for (int side = 0; side < 2; side++) + { + Arm a = _arms[side]; + Vector3 sh = s.Shoulder[side], el = s.Elbow[side], wr = s.Wrist[side]; + Vector3 armDir = wr - sh; + armDir = armDir.LengthSquared() > 1e-8f ? armDir.Normalized() : chest * Vector3.Down; + Vector3 hinge = s.Pole[side].Cross(armDir); + hinge = hinge.LengthSquared() > 1e-8f ? hinge.Normalized() : chest.X; + Vector3 up = (el - sh).Normalized(); + Vector3 fore = (wr - el).Normalized(); + _girdle[side] = chest.Transposed() * (sh - (s.UpperChest + chest.X * ((side == 0 ? -1f : 1f) * Profile.ShoulderHalfWidth))); + _targets[a.Upper] = new Transform3D(BoneBasis(up, hinge), sh + up * (a.UpperLen * 0.5f)); + _targets[a.Fore] = new Transform3D(BoneBasis(fore, hinge), el + fore * (a.ForeLen * 0.5f)); + _targets[a.Hand] = (side == 0 ? left : right).Orthonormalized(); + + if (physicalArms) + { + // Re-aim the arm from the PHYSICAL shoulder. Horizontally the body may be held away from + // the camera (pressed against a ledge, leaning over a table): the arm reaches for the + // real hand. Vertically a sagging body keeps its arm pose (the aim point moves with + // the shoulder), so straight real arms stay straight virtual arms — pillars that hold + // you up when crawling or press you up a ledge (the hand guide adds the rest). + // Only a horizontal TRANSLATION of the reach: solved in the chest TARGET frame (the + // muscles stay local). Aiming in the physical chest frame made the shoulder muscles + // feed chest rotation back into the chest — a slow twist that tracking jitter + // excited into "chest slipping out of control" (owner playtest 2.1). + Transform3D chestPhys = Chest.GlobalTransform.Orthonormalized(); + Vector3 shPhys = chestPhys * (a.Shoulder.AnchorA + _girdle[side]); + Vector3 held = shPhys - sh; + held.Y = 0f; + Transform3D ctrl = _targets[a.Hand]; + ArmSolver solver = _armSolvers[side]; + solver.Solve(sh, ctrl * new Vector3(0f, 0f, WristOffset) - held, chest, chest.X * (side == 0 ? -1f : 1f), + ctrl.Basis, a.UpperLen, a.ForeLen, dt); + Vector3 d = solver.Wrist - sh; + Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chest.X; + h = h.LengthSquared() > 1e-8f ? h.Normalized() : chest.X; + Vector3 u = (solver.Elbow - sh).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized(); + _targets[a.Upper] = new Transform3D(BoneBasis(u, h), sh + u * (a.UpperLen * 0.5f)); + _targets[a.Fore] = new Transform3D(BoneBasis(f, h), solver.Elbow + f * (a.ForeLen * 0.5f)); + } + } + } + + private readonly ArmSolver[] _armSolvers = { new(), new() }; + + public Transform3D Target(RigidBody3D body) => _targets.TryGetValue(body, out Transform3D t) ? t : body.GlobalTransform; + + // ---------------------------------------------------------------- per tick + + public void Step(TrueBodySolver s, Transform3D left, Transform3D right, Vector3 leftVelocity, Vector3 rightVelocity, float floorY, float dt) + { + if (!Built) + return; + _dt = dt; + _floorYForHead = floorY; + ComputeTargets(s, left, right, physicalArms: true, dt); + foreach (RigidBody3D b in _bodies) + { + if (BodyMath.ResetIfNonFinite(b, Target(b), "PlayerRagdoll")) + { + Snaps++; + SnapToPose(s, left, right); + return; + } + } + + Transform3D pelvisTarget = _targets[Pelvis]; + if (Pelvis.GlobalPosition.DistanceTo(pelvisTarget.Origin) > ResetDistance) + { + GD.Print("PlayerRagdoll: body too far from the pose, snapped to it"); + Snaps++; + SnapToPose(s, left, right); + return; + } + + Muscle(_spine, _targets[Pelvis].Basis, _targets[Chest].Basis, dt); + Muscle(_neck, _targets[Chest].Basis, _targets[Head].Basis, dt); + if (Strength != _appliedStrength) + ApplyStrength(); + Braced = false; + for (int side = 0; side < 2; side++) + { + Arm a = _arms[side]; + ShoulderDrive(a, side, dt); + ElbowMuscle(a, dt); + HandGuide(a, _targets[a.Hand], side == 0 ? leftVelocity : rightVelocity); + Braced |= IsBraced(a); + } + GroundControl(pelvisTarget, floorY, dt); + HeadGuide(s, dt); + LegsDrive(s); + _legVisuals.Tick(_footY, Grounded, !_legsSolid, s.YawBasis, dt); + } + + // Muscle = PD on the joint's relative rotation toward the relative rotation of the two TARGET + // bases (a local pose, not a world pin), written as the motor's target velocity in frame A. + // Feed-forward = the target's own relative spin, so a moving pose isn't chased with a lag of + // speed/Kp. + private static void Muscle(Ball b, Basis parentTarget, Basis childTarget, float dt) + { + Quaternion current = (Rot(b.Parent) * b.FrameA).Inverse() * (Rot(b.Child) * b.FrameB); + Quaternion target = (parentTarget.Orthonormalized().GetRotationQuaternion() * b.FrameA).Inverse() + * (childTarget.Orthonormalized().GetRotationQuaternion() * b.FrameB); + Vector3 raw = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; + b.Ff = b.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); + b.PrevTarget = target; + b.HasPrev = true; + Vector3 w = (b.Ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin); + SetAngularVelocity(b.Joint, w); + } + + private static void SetAngularVelocity(Generic6DofJoint3D j, Vector3 w) + { + j.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X); + j.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y); + j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z); + } + + private static void SetLinearVelocity(Generic6DofJoint3D j, Vector3 v) + { + j.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X); + j.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y); + j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z); + } + + // Elbow flexion (0 = straight) about the upper arm's hinge axis. + public float ElbowFlex(int side) + { + Arm a = _arms[side]; + Basis u = a.Upper.GlobalTransform.Basis.Orthonormalized(); + Vector3 f = a.Fore.GlobalTransform.Basis.Orthonormalized().Y; + return Mathf.Atan2(u.X.Dot(u.Y.Cross(f)), u.Y.Dot(f)); + } + + private void ElbowMuscle(Arm a, float dt) + { + int side = a == _arms[0] ? 0 : 1; + Vector3 up = _targets[a.Upper].Basis.Y, fore = _targets[a.Fore].Basis.Y; + float target = Mathf.Acos(Mathf.Clamp(up.Dot(fore), -1f, 1f)); + float raw = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f; + a.FlexFf = Mathf.Lerp(a.FlexFf, raw, LowPass(dt, MuscleFeedForwardTau)); + a.PrevFlexTarget = target; + float w = Mathf.Clamp(a.FlexFf + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin); + a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w); + float cap = (w > 0f ? ElbowFlexTorque : ElbowTorque) * Mathf.Max(0.05f, Strength); + if (Mathf.Abs(cap - a.ElbowCap) > 0.5f) + { + a.ElbowCap = cap; + a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, cap); + } + } + + // Head guide (Boneworks/BIMOS drive the head to the HMD): while standing on the ground, a force- + // capped PD pulls the character's eye onto the camera, so voluntary head motion (squats, leans, + // quick looks) doesn't leave the body lagging — a lag the riding view would otherwise turn into + // a permanent play-space offset. Off while airborne/climbing (no levitating by the head) or + // bracing on the hands (mantling, pushing off). + private void HeadGuide(TrueBodySolver s, float dt) + { + Vector3 goal = s.Eye + Vector3.Up * (_footY - _floorYForHead); + Vector3 raw = _haveHeadPrev && dt > 0f ? (goal - _headGoalPrev) / dt : Vector3.Zero; + _headGoalPrev = goal; + _haveHeadPrev = true; + if (raw.Length() > 10f) + raw = Vector3.Zero; + _headGoalVelocity = _headGoalVelocity.Lerp(raw, LowPass(dt, GroundFeedForwardTau)); + // Only on your feet: hands bracing (mantling, pushing yourself) or crawling move the body by + // other means, and the view simply follows it. + if (!Grounded || Climbing || Braced || _jump != JumpPhase.None || _sinceLanding < AbsorbTime) + return; + float w = Mathf.Tau * HeadGuideFrequency; + Vector3 force = ((goal - Eye) * (w * w) + (_headGoalVelocity - Head.LinearVelocity) * (2f * w)) * HeadGuideMass; + Head.ApplyCentralForce(force.LimitLength(HeadGuideForce)); + } + + private Vector3 _headGoalPrev, _headGoalVelocity; + private bool _haveHeadPrev; + private float _floorYForHead; + + private void ApplyStrength() + { + _appliedStrength = Strength; + float k = Mathf.Max(0.05f, Strength); + SetAngularCap(_spine.Joint, SpineTorque * k); + SetAngularCap(_neck.Joint, NeckTorque * k); + foreach (Arm a in _arms) + { + SetAngularCap(a.Shoulder.Joint, ShoulderTorque * k); + a.ShoulderCap = ShoulderTorque * k; + a.ElbowCap = ElbowTorque * k; + SetAngularCap(a.Guide.Joint, HandGuideTorque * k); + a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque * k); + } + } + + private static void SetAngularCap(Generic6DofJoint3D j, float cap) + { + j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + } + + private static void SetLinearCap(Generic6DofJoint3D j, float cap) + { + j.SetParamX(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); + j.SetParamY(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); + j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap); + } + + // A hand pressing on or holding static world (a ledge, a wall, a grabbed bar) — not the floor: + // hands on the floor while crawling don't lift the body. + private static bool IsBraced(Arm a) + { + if (a.Held is StaticBody3D) + return true; + foreach (Node3D n in a.Hand.GetCollidingBodies()) + if (n is StaticBody3D && !n.IsInGroup(PropGrabber.NoGrabGroup)) + return true; + return false; + } + + // The lower-body collider hangs from the actual hips, its bottom never below a step above the + // feet; once it would have to slide far up into the torso (deep crouch, crawl) it stops colliding. + private void LegsDrive(TrueBodySolver s) + { + Transform3D pelvis = Pelvis.GlobalTransform; + Vector3 hip = pelvis * (-_pelvisCenterLocal); + float minCenter = _footY + MaxStepHeight + _legsHeight * 0.5f; + float center = Mathf.Max(hip.Y - _legsHeight * 0.5f, minCenter); + // Tuck (collider off, folded into the pelvis): deep crouch/crawl, or the hands bracing to + // lift the body (mantle, climbing — the knees come up). It STAYS tucked until it has been + // unneeded for a while AND its untucked spot is free: re-enabling inside a ledge face is + // what shoved the body off again and again (owner 2.2 "legs trying to get off the + // ground and instantly coming back"). + bool climbingHold = _arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D; + _lifted = Braced && (Pelvis.GlobalPosition.Y > _supportGoalY + MantleLift || climbingHold); + bool deep = center + _legsHeight * 0.5f >= hip.Y + LegsOffAbove; + bool wantTuck = deep || _lifted || (!_legsSolid && Braced); + _tuckFree = wantTuck ? 0f : _tuckFree + _dt; + Vector3 untucked = new(hip.X, center, hip.Z); + bool solid = _legsSolid ? !wantTuck : _tuckFree > UntuckDelay && LegsSpaceFree(untucked, s.YawBasis); + if (solid != _legsSolid) + { + _legsSolid = solid; + // The shape itself is disabled: Box3D keeps existing contacts when only the layer changes. + Legs.GetChild(0).SetDeferred(CollisionShape3D.PropertyName.Disabled, !solid); + } + if (!solid) + center = hip.Y; // tucked into the pelvis, out of the way + Vector3 desired = new(hip.X, center, hip.Z); + Quaternion frameA = Rot(Pelvis) * _legsDrive.FrameA; + // Speed-limited: the legs dropping back out of the tuck at full drive speed kicked the pelvis + // up (the reaction) just as a landing dip was rising — a 5 cm bounce. + SetLinearVelocity(_legsDrive.Joint, frameA.Inverse() * ((desired - Legs.GlobalPosition) * LegsKp).LimitLength(LegsMaxSpeed)); + Quaternion target = (Rot(Pelvis) * _legsDrive.FrameA).Inverse() * (s.YawBasis.GetRotationQuaternion() * _legsDrive.FrameB); + Quaternion current = frameA.Inverse() * (Rot(Legs) * _legsDrive.FrameB); + SetAngularVelocity(_legsDrive.Joint, (BodyMath.RotationError(target, current) * LegsKp).LimitLength(MuscleMaxSpin)); + } + + // Shoulder = one drive: its linear motor keeps the humeral head on the chest's shoulder point + // shifted by the True Body girdle offset (shrug/protraction), its angular motor is the + // shoulder muscle (same PD + feed-forward as Muscle, parent = chest). + private void ShoulderDrive(Arm a, int side, float dt) + { + Drive d = a.Shoulder; + Transform3D chest = Chest.GlobalTransform.Orthonormalized(); + Vector3 desired = chest * (d.AnchorA + _girdle[side]); + Vector3 err = desired - a.Upper.GlobalTransform * d.AnchorB; + Quaternion frameA = chest.Basis.GetRotationQuaternion() * d.FrameA; + SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp)); + + Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB); + // Local muscle: relative to the chest's TARGET pose (never to the physical chest — that + // closes a rotation feedback loop through the arms). + Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse() + * (_targets[a.Upper].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameB); + Vector3 raw = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero; + d.Ff = d.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau)); + d.PrevTarget = target; + d.HasPrev = true; + Vector3 spin = (d.Ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin); + SetAngularVelocity(d.Joint, spin); + // Which way is the muscle moving the arm? Rotating the bone direction downward = pulling. + Vector3 bone = (a.Upper.GlobalTransform.Basis.Y).Normalized(); + Vector3 boneMotion = (frameA * spin).Cross(bone); + float cap = (boneMotion.Y < 0f ? ShoulderPullTorque : ShoulderTorque) * Mathf.Max(0.05f, Strength); + if (Mathf.Abs(cap - a.ShoulderCap) > 0.5f) + { + a.ShoulderCap = cap; + SetAngularCap(d.Joint, cap); + } + } + + // Force-capped pull of the hand onto the controller, reacting into the chest (arm muscles). + private void HandGuide(Arm a, Transform3D target, Vector3 targetVelocity) + { + Transform3D chest = Chest.GlobalTransform; + Vector3 anchor = chest * a.Guide.AnchorA; + Vector3 anchorVelocity = Chest.LinearVelocity + Chest.AngularVelocity.Cross(anchor - Chest.GlobalPosition); + Vector3 err = target.Origin - a.Hand.GlobalPosition; + Vector3 desired = (targetVelocity + err * HandGuideKp).LimitLength(HandGuideMaxSpeed); + Quaternion frameA = Rot(Chest) * a.Guide.FrameA; + Vector3 shoulder = a.Upper.GlobalTransform * a.Shoulder.AnchorB; + Vector3 axis = a.Hand.GlobalPosition - shoulder; + // Axial share of the error, with a 3 cm floor so a nearly-reached target's noisy error + // direction can't flip a held load into "structural". + // PUSH only (the hand wants to go further from the shoulder — straight arms pressing on a + // ledge): bone in compression. Pulling (shortening the arm: pull-ups, climbing) is muscle + // work — the asymmetric shoulder/elbow muscles do it, and one arm can't lift the body. + float align = axis.LengthSquared() > 1e-8f ? Mathf.Max(0f, err.Dot(axis.Normalized())) / (err.Length() + StructuralDeadband) : 0f; + // Structure only helps against something: a hand in free air reaching past its arm's + // length must not haul the whole body after it. + if (a.Held == null && a.Hand.GetContactCount() == 0) + align = 0f; + float cap = Mathf.Lerp(HandGuideForce, HandStructuralForce, align) * Mathf.Max(0.05f, Strength); + if (Mathf.Abs(cap - a.GuideCap) > 1f) + { + a.GuideCap = cap; + SetLinearCap(a.Guide.Joint, cap); + } + SetLinearVelocity(a.Guide.Joint, frameA.Inverse() * (desired - anchorVelocity)); + // Angular: hand rotation onto the controller rotation, as a relative spin vs the chest. + Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand)); + Vector3 wWorld = (rotErr * HandGuideAngularKp).LimitLength(MuscleMaxSpin * 2f) - Chest.AngularVelocity; + SetAngularVelocity(a.Guide.Joint, frameA.Inverse() * wWorld); + } + + // "Leg strength": a stiff, mass-scaled support holding the pelvis at its pose height above the + // ground and carrying it after the head horizontally (friction-limited), plus an upright/yaw + // balance motor (world anchor → pelvis). No ground → no support (falls); balance weakens in the air. + private void GroundControl(Transform3D target, float floorY, float dt) + { + Vector3 pos = Pelvis.GlobalPosition; + float hipAboveFloor = target.Origin.Y - floorY; + var query = PhysicsRayQueryParameters3D.Create(pos, pos + Vector3.Down * (hipAboveFloor + GroundProbeExtra + 0.5f), WorldMask); + Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query); + bool wasGrounded = Grounded; + Grounded = false; + _groundBody = null; + if (hit.Count > 0) + { + Vector3 point = (Vector3)hit["position"]; + Vector3 normal = (Vector3)hit["normal"]; + _groundBody = GodotObject.InstanceFromId((ulong)hit["collider_id"]) as RigidBody3D; // Box3D: "collider" is null + _groundPoint = point; + // Standing (or walking down a step) the legs keep contact over GroundProbeExtra; falling, + // they touch down only within their reach — the rest of the fall is stopped by the legs. + float reach = wasGrounded && _jump != JumpPhase.Flight ? GroundProbeExtra : LandingReach; + bool inReach = pos.Y - point.Y < hipAboveFloor + reach; + bool steep = normal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees)); + if (_jump == JumpPhase.Flight && Pelvis.LinearVelocity.Y > 0f) + { + // Still rising off the ground: nothing to stand on yet. + } + else if (inReach && steep && wasGrounded && point.Y <= _footY + MaxStepHeight) + { + // The foot of a too-steep slope: keep standing on the ground we're on (the legs + // collider stops us); dropping support here would collapse the body. + Grounded = true; + } + else if (inReach && !steep) + { + // Step rule: ground more than a step above the feet is only stood on once the + // hips are already well above it (climbed/mantled/landed there) — otherwise it's an + // obstacle the legs collider holds us back from, and the feet stay where they were. + // Higher ground climbed onto must be under the hips' back edge too (not just a + // pelvis centre hanging over the lip). + bool clear = point.Y <= _footY + MaxStepHeight || GroundBehindAt(pos, point.Y); + bool reachable = point.Y <= _footY + MaxStepHeight + || ((!wasGrounded || _lifted || pos.Y - point.Y > 0.6f * hipAboveFloor) && clear); + Grounded = true; + if (reachable) + _footY = point.Y; + } + } + + // Feed-forward from the GOAL (pose + ground offset): when the play-space floor follows the + // ground, the pose target and the ground offset move by the same amount in opposite + // directions — differentiating the pose alone would launch the body. + Vector3 goalNow = target.Origin + Vector3.Up * (_footY - floorY); + Vector3 rawVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero; + _pelvisTargetPrev = goalNow; + _havePelvisPrev = true; + if (rawVelocity.Length() > 10f) + rawVelocity = Vector3.Zero; + _goalVelocity = _goalVelocity.Lerp(rawVelocity, LowPass(dt, GroundFeedForwardTau)); + // Held back = the hips can't get to where the pose wants them (knee-high box, table edge, + // wall). Measured at the hips, not the head: the body tips over an obstacle, so the head + // may be right where the camera is while the hips are stuck. + Vector3 hipLag = new(target.Origin.X - pos.X, 0f, target.Origin.Z - pos.Z); + HeldBack = Grounded && hipLag.Length() > FreeSlack ? hipLag.Normalized() : Vector3.Zero; + Vector3 targetVelocity = _goalVelocity; + + float weight = BodyMass * 9.8f; + if (Grounded && !wasGrounded && Pelvis.LinearVelocity.Y < -1.5f) + _sinceLanding = 0f; + _sinceLanding += dt; + UpdateJump(dt, wasGrounded); + if (_jump == JumpPhase.Push) + Grounded = true; // the feet push off the ground they were on (also over a just-left edge) + if (Grounded) + { + // Pose heights are relative to the tracking floor; stand on the ground actually under us. + Vector3 goal = target.Origin + Vector3.Up * (GroundY - floorY); + _supportGoalY = goal.Y; + if (_jump == JumpPhase.Dip) + goal.Y = Mathf.Min(goal.Y, _jumpFromY); + float w = Mathf.Tau * GroundFrequency; + Vector3 accel = (goal - pos) * (w * w) + (targetVelocity - Pelvis.LinearVelocity) * (2f * GroundDamping * w); + Vector3 force = accel * BodyMass; + // Vertical as a rate-limited velocity servo: full leg strength brakes a landing, but + // the legs raise the body at most MaxRiseSpeed — they can push, not pull, so a hard push + // toward a higher stance (onto a ledge) would otherwise overshoot into a jump. + // Rising, the legs can only stop the body by letting gravity do it (support ≥ 0): ease the + // rise so it can still be stopped at the stance with ~0.7 g, or it overshoots (a bounce). + float below = goal.Y - pos.Y; + float stoppable = below > 0f ? Mathf.Sqrt(2f * 0.7f * 9.8f * below) : 0f; + float riseVelocity = Mathf.Clamp(below * w, -3f, Mathf.Min(MaxRiseSpeed, stoppable)) + targetVelocity.Y; + float verticalAccel = (riseVelocity - Pelvis.LinearVelocity.Y) * (2f * GroundDamping * w); + float support = Mathf.Clamp(verticalAccel * BodyMass + weight, 0f, LegStrength * weight); + if (_jump == JumpPhase.Push) + support = JumpForce * weight; + float carry = Mathf.Max(GroundFriction * support, _legsSolid ? 0f : CrawlCarry); + // Hips held back by an obstacle: lean on it, don't bulldoze — full leg force tipped the + // body over knee-high boxes and steep slopes (and the riding view tipped with it). + if (HeldBack != Vector3.Zero) + carry = Mathf.Min(carry, BlockedPush); + Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(carry); + Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support); + // Newton's third law: standing on something movable (seesaw, plank, crate) pushes it + // down — the feet aren't physical, so the reaction is applied where the ground ray hit. + if (_groundBody != null && IsInstanceValid(_groundBody)) + _groundBody.ApplyForce(-(horizontal + Vector3.Up * support), _groundPoint - _groundBody.GlobalPosition); + } + else if (Braced) + { + // Hanging/mantling on the hands: the hips still drift after the head a little, so you can + // shift your weight over a ledge you're pressing yourself up onto. + Vector3 lead = target.Origin - pos; + lead.Y = 0f; + Vector3 v = Pelvis.LinearVelocity; + v.Y = 0f; + Pelvis.ApplyCentralForce(((lead * 20f - v * 8f) * BodyMass).LimitLength(BracedAirControl)); + } + + Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(Pelvis)); + Vector3 spin = (rotErr * BalanceKp).LimitLength(MuscleMaxSpin); + SetAngularVelocity(_balance.Joint, _balance.FrameA.Inverse() * spin); // anchor = world frame + float cap = BalanceTorque * (Grounded ? 1f : AirBalanceShare); + if (cap != _balanceCap) + { + _balanceCap = cap; + _balance.Joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + _balance.Joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + _balance.Joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap); + } + } + + // Jump state: request → Dip (countermovement, the support goal drops) → Push (full leg force up to + // extension) → Flight (no ground until falling again) → None on touchdown. + private void UpdateJump(float dt, bool wasGrounded) + { + _airT = Grounded ? 0f : _airT + dt; + _jumpRequest -= dt; + _jumpT += dt; + float pos = Pelvis.GlobalPosition.Y; + float top = _footY + _standingHip + JumpExtension; + switch (_jump) + { + case JumpPhase.None: + if (_jumpRequest > 0f && (Grounded || _airT < CoyoteTime) && !Climbing) + { + _jumpRequest = 0f; + _jumpT = 0f; + // Stroke at least JumpDip + JumpExtension: from standing, dip first. + _jumpFromY = Mathf.Max(top - JumpMaxStroke, Mathf.Min(pos, top - JumpDip - JumpExtension)); + _jump = pos > _jumpFromY + 0.02f && Grounded ? JumpPhase.Dip : JumpPhase.Push; + _jumpFromY = Mathf.Min(_jumpFromY, pos); + } + break; + case JumpPhase.Dip: + if (pos <= _jumpFromY + 0.02f || _jumpT > 0.2f || !Grounded) + { + _jump = JumpPhase.Push; + _jumpFromY = pos; + _jumpT = 0f; + } + break; + case JumpPhase.Push: + if (pos >= top || pos - _jumpFromY >= JumpMaxStroke || _jumpT > 0.35f) + { + _jump = JumpPhase.Flight; + _jumpT = 0f; + } + break; + case JumpPhase.Flight: + if (Grounded && _jumpT > 0.05f) + _jump = JumpPhase.None; + break; + } + } + + private Drive _balance; + private float _balanceCap; + + // ---------------------------------------------------------------- play space + + // How the driver should move its play-space origin after the last physics step. ONE rule, always, + // in all directions (Boneworks/BIMOS: the rig rides the body): the camera rides the character's + // eye. Head motion leads the body by at most ViewDeadzone (eased) / MaxViewLead (hard) — less + // than the head box reaches in front of the eye, so the view stays inside the head. A sphere + // swept eye→camera snaps the view back at once when geometry is in the way. + // This does NOT feed back into the pose: the True Body reads the head relative to the play-space + // floor (crouch = local head height) and ground control re-bases its goal onto the real ground + // (footY − floorY), so moving the play space vertically changes neither. (2.3 split height off + // to a separate floor-follow out of fear of that loop; the two then fought over the same height, + // and landings left the view hanging above a sinking body — owner 2.4.) + public Vector3 OriginCorrection(Vector3 camera, float floorY, float dt) + { + if (!Built) + return Vector3.Zero; + Vector3 eye = Eye; + Vector3 lead = camera - eye; + Vector3 correction = Vector3.Zero; + if (lead.LengthSquared() > 1e-8f) + { + _cameraQuery ??= new PhysicsShapeQueryParameters3D { Shape = new SphereShape3D { Radius = CameraRadius }, CollisionMask = WorldMask }; + _cameraQuery.Transform = new Transform3D(Basis.Identity, eye); + _cameraQuery.Motion = lead; + float[] fractions = GetWorld3D().DirectSpaceState.CastMotion(_cameraQuery); + float safe = fractions.Length > 0 ? fractions[0] : 1f; + if (safe < 1f) + correction = -lead * (1f - safe); + Vector3 rest = lead + correction; + if (rest.Length() > ViewDeadzone) + correction -= rest * (1f - ViewDeadzone / rest.Length()) * (1f - Mathf.Exp(-dt / ViewFollowTau)); + rest = lead + correction; + if (rest.Length() > MaxViewLead) + correction -= rest * (1f - MaxViewLead / rest.Length()); + } + // Slow re-centering of the play-space floor onto the real ground while standing: removes the + // small height offsets the riding view picks up (a transient lag, a landing). The pose and the + // head guide are re-based on the real ground, so this only brings the camera onto the eye — + // the same direction as the follow above, never against it. + if (Grounded) + correction.Y += (_footY - floorY) * (1f - Mathf.Exp(-dt / FloorRecenterTau)); + // The pose target moves with the play space: a correction is not intended motion, so it + // must not show up in ground control's feed-forward (that loop ran away at 7 m/s). + // Horizontally only — vertically the goals are re-based on the real ground (− floorY), so + // they don't move with the play space; shifting them too turned the view riding a landing + // dip into a ~1 m/s upward feed-forward (a 15 cm bounce). + Vector3 flat = new(correction.X, 0f, correction.Z); + _pelvisTargetPrev += flat; + _headGoalPrev += flat; + return correction; + } + + private PhysicsShapeQueryParameters3D _cameraQuery; + + // Second ground ray 12 cm behind the hips (toward the body's back): ground at least `groundY`. + private bool GroundBehindAt(Vector3 pelvisPos, float groundY) + { + Vector3 back = Pelvis.GlobalTransform.Basis.Z; + back.Y = 0f; + back = back.LengthSquared() > 1e-6f ? back.Normalized() * 0.12f : Vector3.Zero; + var query = PhysicsRayQueryParameters3D.Create(pelvisPos + back, pelvisPos + back + Vector3.Down * 3f, WorldMask); + Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query); + return hit.Count > 0 && ((Vector3)hit["position"]).Y > groundY - 0.05f; + } + + public void TeleportBy(Transform3D delta) + { + if (!Built) + return; + foreach (RigidBody3D b in _bodies) + { + b.GlobalTransform = delta * b.GlobalTransform; + b.LinearVelocity = delta.Basis * b.LinearVelocity; + b.AngularVelocity = delta.Basis * b.AngularVelocity; + } + _pelvisTargetPrev = delta * _pelvisTargetPrev; + _footY += delta.Origin.Y; + foreach (RigidBody3D b in _bodies) + b.ResetPhysicsInterpolation(); + } + + public void SnapToPose(TrueBodySolver s, Transform3D left, Transform3D right) + { + ComputeTargets(s, left, right); + foreach (RigidBody3D b in _bodies) + { + b.GlobalTransform = Target(b); + b.LinearVelocity = Vector3.Zero; + b.AngularVelocity = Vector3.Zero; + } + // The hands sit at the wrist end of the solved arm, not at a possibly unreachable controller. + for (int side = 0; side < 2; side++) + { + Arm a = _arms[side]; + Basis hb = _targets[a.Hand].Basis; + a.Hand.GlobalTransform = new Transform3D(hb, s.Wrist[side] - hb * new Vector3(0f, 0f, WristOffset)); + } + _havePelvisPrev = false; + foreach (RigidBody3D b in _bodies) + b.ResetPhysicsInterpolation(); + } + + // Collision exceptions between a held prop and that arm's segments (its hand is excluded by + // the grab weld itself) — a forearm wrapped around a held object must not fight it. + public void SetHeld(int side, PhysicsBody3D prop) + { + if (!Built) + return; + Arm a = _arms[side]; + if (a.Held != null && IsInstanceValid(a.Held) && a.Held != _arms[1 - side].Held) + { + a.Upper.RemoveCollisionExceptionWith(a.Held); + a.Fore.RemoveCollisionExceptionWith(a.Held); + } + a.Held = prop; + if (prop != null) + { + a.Upper.AddCollisionExceptionWith(prop); + a.Fore.AddCollisionExceptionWith(prop); + } + } } diff --git a/scripts/PlayerRig.cs b/scripts/PlayerRig.cs index c4241c4..e26b0c6 100644 --- a/scripts/PlayerRig.cs +++ b/scripts/PlayerRig.cs @@ -4,7 +4,8 @@ using Godot; // Boneworks-style stick locomotion and the play-space reconciliation. Runs everything in one // physics tick, in this order: // 1. discrete origin jumps made by other code (spawn, teleport markers) carry the body along; -// 2. stick: left = move (head-relative), right X = turn (snap by default, SmoothTurn for smooth); +// 2. stick: left = move (head-relative), right X = turn (snap by default, SmoothTurn for smooth), +// right Y = crouch (down lowers, up raises — RagdollDriver.StickCrouch); right A = jump; // 3. the ragdoll's OriginCorrection (push-back at walls, floor follows the ground, falls); // 4. True Body solve from the corrected camera/controllers, then the ragdoll's muscles. // Waits for the calibrated BodyProfile like everything else; rebuilds on recalibration. @@ -18,6 +19,8 @@ public partial class PlayerRig : Node public const float SmoothTurnSpeed = 2.5f; // rad/s public const float SnapTurnThreshold = 0.7f; public const float SnapTurnRearm = 0.4f; + public const float StickCrouchSpeed = 1.2f; // m/s at full stick + public const float StickCrouchHeadroom = 0.5f; // the stick can lower the eyes to this height (a crawl) public const float JumpDistance = 0.5f; // an origin move this big in one tick = teleport, not motion public const float JumpAngle = 0.1f; // rad private const float GhostLagDistance = 0.1f; @@ -33,9 +36,17 @@ public partial class PlayerRig : Node private bool _haveLastOrigin; private Vector3 _moveVelocity; private bool _snapArmed = true; + private bool _jumpPrev; private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2]; private bool _rebuild = true; private RagdollDriver _driver; + // Render interpolation of the play space (smooth-camera bible, VR edition): physics moves the + // origin every tick, but the body is DRAWN interpolated between the last two ticks. A camera + // stepping with the raw origin while the body glides a tick behind looks like the body is in + // two places at once when you move and look at it. So physics owns _physOrigin and every + // frame the visible origin is the same interpolation of it the bodies get. + private Transform3D _physOrigin, _prevPhysOrigin, _shownOrigin; + private bool _interpolateOrigin; public PlayerRagdoll Ragdoll => _ragdoll; public static bool Spectator { get; private set; } @@ -70,6 +81,8 @@ public partial class PlayerRig : Node public override void _Ready() { _instance = this; + ProcessPriority = -100; // place the interpolated origin before anything reads it this frame + ProcessPhysicsPriority = -100; // restore the physics origin before anything uses it this tick _origin = GetNode("../XROrigin3D"); _camera = GetNode("../XROrigin3D/XRCamera3D"); _left = GetNode("../XROrigin3D/LeftHand"); @@ -114,8 +127,10 @@ public partial class PlayerRig : Node if (profile == null || Spectator) { _haveLastOrigin = false; + _interpolateOrigin = false; return; } + RestorePhysicsOrigin(); // 1. Someone else moved the play space discretely: bring the body along. Transform3D origin = _origin.GlobalTransform; @@ -134,10 +149,34 @@ public partial class PlayerRig : Node _origin.GlobalTransform = o; _lastOrigin = _origin.GlobalTransform; _haveLastOrigin = true; + _prevPhysOrigin = _interpolateOrigin && !IsJump(_physOrigin, _lastOrigin) ? _physOrigin : _lastOrigin; + _physOrigin = _shownOrigin = _lastOrigin; + _interpolateOrigin = true; + } + + // The origin on screen is interpolated; physics must run on the real one. Anyone else who moved + // the origin since (teleport marker, menu) is respected as the new real one. + private void RestorePhysicsOrigin() + { + if (!_interpolateOrigin) + return; + Transform3D current = _origin.GlobalTransform; + if (current.IsEqualApprox(_shownOrigin)) + _origin.GlobalTransform = _physOrigin; + else + _physOrigin = _prevPhysOrigin = current; } public override void _Process(double delta) { + if (_interpolateOrigin && !Spectator) + { + Transform3D current = _origin.GlobalTransform; + if (!current.IsEqualApprox(_shownOrigin)) + _physOrigin = _prevPhysOrigin = current; // moved by someone else: show it as is + _shownOrigin = _prevPhysOrigin.InterpolateWith(_physOrigin, (float)Engine.GetPhysicsInterpolationFraction()); + _origin.GlobalTransform = _shownOrigin; + } for (int i = 0; i < 2; i++) { XRController3D c = i == 0 ? _left : _right; @@ -178,9 +217,9 @@ public partial class PlayerRig : Node Vector3 desired = Vector3.Zero; if (move.Length() > StickDeadzone) { - Vector3 fwd = -_camera.GlobalTransform.Basis.Z; - fwd.Y = 0f; - fwd = fwd.LengthSquared() > 1e-6f ? fwd.Normalized() : Vector3.Forward; + Vector3 fwd = BodyMath.HeadFacing(_camera.GlobalTransform.Basis.Orthonormalized()); + if (fwd == Vector3.Zero) + fwd = -Solver.YawBasis.Z; Vector3 rightDir = fwd.Cross(Vector3.Up); float mag = Mathf.Clamp((move.Length() - StickDeadzone) / (1f - StickDeadzone), 0f, 1f); desired = (fwd * move.Y + rightDir * move.X).Normalized() * (mag * MoveSpeed); @@ -192,6 +231,23 @@ public partial class PlayerRig : Node if (_moveVelocity != Vector3.Zero) _origin.GlobalPosition += _moveVelocity * dt; + // Jump: right A, on the press. + bool jump = _right.IsButtonPressed("ax_button"); + if (jump && !_jumpPrev) + _ragdoll.Jump(); + _jumpPrev = jump; + + // Crouch: right stick held down lowers the body, up raises it back (the stick's dominant + // axis decides between crouch and turn). + bool vertical = Mathf.Abs(turn.Y) > Mathf.Abs(turn.X); + if (vertical && Mathf.Abs(turn.Y) > StickDeadzone) + { + float max = Mathf.Max(0f, BodyProfile.Current.EyeHeight - StickCrouchHeadroom); + _driver.StickCrouch = Mathf.Clamp(_driver.StickCrouch - turn.Y * StickCrouchSpeed * dt, 0f, max); + } + if (vertical) + turn.X = 0f; + // Turn about the head. float angle = 0f; if (SmoothTurn) diff --git a/scripts/RagdollDriver.cs b/scripts/RagdollDriver.cs index 8763a8b..f80eb6c 100644 --- a/scripts/RagdollDriver.cs +++ b/scripts/RagdollDriver.cs @@ -68,6 +68,11 @@ public sealed class RagdollDriver public TrueBodySolver Solver; public PlayerRagdoll Ragdoll; public bool RebuildRequested = true; + // Stick crouch (right stick down), metres: the play-space floor is treated as this far ABOVE where + // it is. The True Body then reads the head as that much lower (crouch pose), ground control and + // the view's re-centering settle the play space that far below the ground — so the camera rides + // down with the crouching body, and nothing feeds back (same play-space-relative rule as 2.5). + public float StickCrouch; private double _now; // origin = play space (modified by the correction); raw* = origin-local tracked poses as last @@ -79,15 +84,16 @@ public sealed class RagdollDriver _now += dt; Transform3D head = Head.Sample(rawHead, _now), left = Left.Sample(rawLeft, _now), right = Right.Sample(rawRight, _now); if (Ragdoll.Built && !RebuildRequested) - origin.Origin += Ragdoll.OriginCorrection((origin * head).Origin, origin.Origin.Y, dt); + origin.Origin += Ragdoll.OriginCorrection((origin * head).Origin, origin.Origin.Y + StickCrouch, dt); + float floorY = origin.Origin.Y + StickCrouch; Transform3D h = origin * head, l = origin * left, r = origin * right; - Solver.Solve(profile, h, l, r, origin.Origin.Y, dt); + Solver.Solve(profile, h, l, r, floorY, dt); if (RebuildRequested || !Ragdoll.Built) { Ragdoll.Build(profile, Solver, l, r); RebuildRequested = false; } Vector3 lv = origin.Basis * Left.Velocity + moveVelocity, rv = origin.Basis * Right.Velocity + moveVelocity; - Ragdoll.Step(Solver, l, r, lv, rv, origin.Origin.Y, dt); + Ragdoll.Step(Solver, l, r, lv, rv, floorY, dt); } } diff --git a/scripts/RagdollSpike.cs b/scripts/RagdollSpike.cs index c9bad63..7ec3a7b 100644 --- a/scripts/RagdollSpike.cs +++ b/scripts/RagdollSpike.cs @@ -38,6 +38,7 @@ public partial class RagdollSpike : Node3D private Transform3D Right => _origin * Tracked(2, _rightLocal); private bool _realInput = true, _noiseOn = true, _holdOn = true; + private string[] _only; // `-- only=jump,fall`: run just the phases whose names contain one of these private const float TrackingHz = 90f; private readonly Transform3D[] _held = new Transform3D[3]; private readonly long[] _heldFrame = { -1, -1, -1 }; @@ -71,6 +72,8 @@ public partial class RagdollSpike : Node3D _noiseOn = false; if (arg == "nohold") _holdOn = false; + if (arg.StartsWith("only=")) + _only = arg[5..].Split(','); } GD.Print($"RAGDOLL: input mode {(_realInput ? "REAL (90 Hz tracking, jitter)" : "ideal")} at {Engine.PhysicsTicksPerSecond} Hz physics"); StaticBody3D floor = MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29); @@ -106,11 +109,18 @@ public partial class RagdollSpike : Node3D _phases.Add(("holds poses (stand, crouch, deep crouch, lean, reach, kneel) without view/body drift", Rest, TickPoseHold, 18f, FinishPoseHold)); _phases.Add(("hanging from a bar keeps the view on the body", () => SetupBarHang(), TickHang, 4f, FinishHang)); _phases.Add(("climbs a wall hand over hand (grab, pull, regrab higher)", SetupClimbWall, TickClimbWall, 8f, FinishClimbWall)); - _phases.Add(("walks off a 1 m ledge and lands without sinking away from the view", SetupLedgeTop, TickDrop, 4f, FinishDrop)); + _phases.Add(("walks off a 1 m ledge and lands: a dip the view follows, no bounce", () => SetupLedgeTop(1f), TickDrop, 4f, () => FinishDrop(1f))); + _phases.Add(("big fall (5 m): legs fold fully, then it recovers", () => SetupLedgeTop(5f), TickDrop, 5f, () => FinishDrop(5f))); + _phases.Add(("standing jump", Rest, t => TickJump(0f), 4f, () => FinishJump(crouched: false))); + _phases.Add(("jump from a crouch goes higher", Rest, t => TickJump(0.45f), 4.5f, () => FinishJump(crouched: true))); + _phases.Add(("looks down past vertical while crouched (body doesn't flip around)", Rest, TickLookDown, 5f, FinishLookDown)); + _phases.Add(("stick crouch lowers the body and the view, and stands back up", Rest, TickStickCrouch, 6f, FinishStickCrouch)); _phases.Add(("standing on a seesaw tips it (body weight reaches movable ground)", SetupSeesaw, _ => { }, 3f, FinishSeesaw)); _phases.Add(("playground: swing board hangs, seesaw on its pivot, big plank grabbable", SetupPlayground, _ => { }, 2f, FinishPlayground)); _phases.Add(("walks up an 18° ramp", () => SetupRamp(18f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(true))); _phases.Add(("can't walk up a 40° ramp", () => SetupRamp(40f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(false))); + if (_only != null) + _phases.RemoveAll(ph => ph.Name != "hinge sign" && !Array.Exists(_only, o => ph.Name.Contains(o))); Next(); } @@ -153,6 +163,8 @@ public partial class RagdollSpike : Node3D _finite = true; _measureFrom = 1f; _stickVelocity = Vector3.Zero; + if (_driver != null) + _driver.StickCrouch = 0f; _phases[_phase].Setup(); _snapsAtStart = _ragdoll.Snaps; } @@ -662,38 +674,176 @@ public partial class RagdollSpike : Node3D } - // Start standing on a 1 m platform whose edge is 0.5 m ahead. - private void SetupLedgeTop() + // Start standing on a platform whose edge is 0.5 m ahead. + private void SetupLedgeTop(float height) { Rest(); - _origin = new Transform3D(Basis.Identity, new Vector3(0f, 1f, 0f)); - _stage.AddChild(MakeStatic(new Vector3(3f, 1f, 3f), new Vector3(0f, 0.5f, 1f), 29)); + _origin = new Transform3D(Basis.Identity, new Vector3(0f, height, 0f)); + _stage.AddChild(MakeStatic(new Vector3(3f, height, 3f), new Vector3(0f, height * 0.5f, 1f), 29)); + _dropEyeErr = _dropEyeErrLate = _dropRebound = 0f; + _dropMinPelvis = float.MaxValue; + _dropStand = 0f; } - private float _dropEyeErr, _dropMinPelvis = float.MaxValue; + private float _dropEyeErr, _dropEyeErrLate, _dropMinPelvis = float.MaxValue, _dropStand, _dropRebound; private void TickDrop(float dt) { _stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero; - if (_ragdoll.Built && !_needBuild && _t > 0.5f) + if (!_ragdoll.Built || _needBuild) + return; + float pelvis = _ragdoll.Pelvis.GlobalPosition.Y; + if (_t < 0.5f) + _dropStand = pelvis - _ragdoll.GroundY; // standing hips above the feet + else { _dropEyeErr = Mathf.Max(_dropEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin)); if (_ragdoll.Grounded && _ragdoll.GroundY < 0.5f) - _dropMinPelvis = Mathf.Min(_dropMinPelvis, _ragdoll.Pelvis.GlobalPosition.Y); + { + if (pelvis < _dropMinPelvis) + { + _dropMinPelvis = pelvis; + _dropRebound = pelvis; + } + _dropRebound = Mathf.Max(_dropRebound, pelvis); + } + if (_dropMinPelvis < float.MaxValue && _ragdoll.Pelvis.LinearVelocity.Y > -0.5f) + _dropEyeErrLate = Mathf.Max(_dropEyeErrLate, _ragdoll.Eye.DistanceTo(Camera.Origin)); } } - private bool FinishDrop() + private bool FinishDrop(float height) { - GD.Print($"RAGDOLL: landing: eye vs camera max {_dropEyeErr:F3} m (limit 0.15), lowest pelvis after touchdown {_dropMinPelvis:F3} m (standing 0.99, limit > 0.7)"); - bool landed = _dropEyeErr < 0.15f && _dropMinPelvis > 0.7f; - _dropEyeErr = 0f; - _dropMinPelvis = float.MaxValue; + float dip = _dropStand - _dropMinPelvis, overshoot = _dropRebound - _dropStand; + // The dip grows with the impact; a big fall folds the legs (hips near the ground). Rebound + // past the stance would be a bounce. The view rides the body throughout. + bool dipOk = height < 2f ? dip > 0.12f && dip < 0.5f : dip > 0.45f; + GD.Print($"RAGDOLL: landing from {height} m: dip {dip:F3} m (stance {_dropStand:F3}), rebound above the stance {overshoot:F3} (limit 0.05), eye vs camera max {_dropEyeErr:F3} / after the impact {_dropEyeErrLate:F3} (limit 0.1)"); + bool landed = dipOk && overshoot < 0.05f && _dropEyeErrLate < 0.1f; if (!landed) return false; GD.Print($"RAGDOLL: drop: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis {_ragdoll.Pelvis.GlobalPosition}, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}"); return _finite && NewSnaps == 0 && Mathf.Abs(_ragdoll.GroundY) < 0.05f && Mathf.Abs(_origin.Origin.Y) < 0.1f - && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > -3f; + && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > -3f + && Mathf.Abs(_ragdoll.Pelvis.GlobalPosition.Y - _ragdoll.GroundY - _dropStand) < 0.05f; + } + + // ---------------------------------------------------------------- jump + + private float _jumpStand, _jumpApex, _jumpEyeErr, _standJumpApex; + private bool _jumpPressed; + + // Optionally squat (eyes `crouch` lower) first, then press jump at 2 s; the player keeps the pose. + private void TickJump(float crouch) + { + float k = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((_t - 0.8f) / 0.8f, 0f, 1f)) * (crouch > 0f ? 1f : 0f); + _headLocal = new Transform3D(new Basis(Vector3.Right, -0.2f * k), new Vector3(0f, EyeHeight - crouch * k, -0.05f * k)); + _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f - crouch * k, -0.3f)); + _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f - crouch * k, -0.3f)); + _measureFrom = float.MaxValue; + if (!_ragdoll.Built || _needBuild) + return; + float pelvis = _ragdoll.Pelvis.GlobalPosition.Y; + if (_t < 0.7f) + { + _jumpStand = pelvis; + _jumpApex = 0f; + _jumpEyeErr = 0f; + _jumpPressed = false; + } + if (_t >= 2f && !_jumpPressed) + { + _jumpPressed = true; + _ragdoll.Jump(); + } + if (_jumpPressed) + { + _jumpApex = Mathf.Max(_jumpApex, pelvis - _jumpStand); + _jumpEyeErr = Mathf.Max(_jumpEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin)); + } + } + + private bool FinishJump(bool crouched) + { + GD.Print($"RAGDOLL: {(crouched ? "crouched" : "standing")} jump: hips rose {_jumpApex:F3} m above the standing stance, eye vs camera max {_jumpEyeErr:F3}, grounded {_ragdoll.Grounded}, jumping {_ragdoll.Jumping}, snaps {NewSnaps}"); + bool ok = _finite && NewSnaps == 0 && _ragdoll.Grounded && !_ragdoll.Jumping && _jumpEyeErr < 0.12f && _origin.Origin.Length() < 0.15f; + if (!crouched) + { + _standJumpApex = _jumpApex; + return ok && _jumpApex > 0.3f && _jumpApex < 0.65f; + } + return ok && _jumpApex > _standJumpApex + 0.1f; + } + + // ---------------------------------------------------------------- looking down past vertical + + private float _lookYaw0, _lookYawMax, _lookChestMax, _lookPitchMax; + + private static float YawOfBody(RigidBody3D b) + { + Vector3 f = BodyMath.HeadFacing(b.GlobalTransform.Basis.Orthonormalized()); + return f == Vector3.Zero ? 0f : BodyMath.YawOf(f); + } + + // Crouch (eyes 1.0 m), then look down to 125° below the horizon (chin on the chest, looking + // behind the feet) and back, hands down near the floor — interacting with stuff on the ground. + private void TickLookDown(float dt) + { + float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1f, 1f)); + float pitch = Mathf.DegToRad(125f) * Mathf.Sin(Mathf.Pi * Mathf.Clamp((_t - 1.5f) / 3f, 0f, 1f)); + _lookPitchMax = Mathf.Max(_lookPitchMax, pitch); + Basis head = new(Vector3.Right, -pitch); + Vector3 pivot = new(0f, Mathf.Lerp(EyeHeight, 1.0f, k) + TrueBodySolver.EyeToNeckPivot.Y, -0.1f * k + TrueBodySolver.EyeToNeckPivot.Z); + _headLocal = new Transform3D(head, pivot - head * TrueBodySolver.EyeToNeckPivot); + _leftLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f)); + _rightLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f)); + _measureFrom = float.MaxValue; + if (!_ragdoll.Built || _needBuild) + return; + if (_t < 1.4f) + { + _lookYaw0 = YawOfBody(_ragdoll.Pelvis); + _lookYawMax = _lookChestMax = _lookPitchMax = 0f; + return; + } + _lookYawMax = Mathf.Max(_lookYawMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Pelvis)))); + _lookChestMax = Mathf.Max(_lookChestMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Chest)))); + } + + private bool FinishLookDown() + { + GD.Print($"RAGDOLL: look down to {Mathf.RadToDeg(_lookPitchMax):F0}°: pelvis yaw change max {Mathf.RadToDeg(_lookYawMax):F1}°, chest {Mathf.RadToDeg(_lookChestMax):F1}° (limit 15), eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}"); + return _finite && NewSnaps == 0 && _lookYawMax < Mathf.DegToRad(15f) && _lookChestMax < Mathf.DegToRad(15f) + && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.08f; + } + + // ---------------------------------------------------------------- stick crouch + + private float _stickEyeLow, _stickOriginLow, _stickEyeErr; + + private void TickStickCrouch(float dt) + { + _measureFrom = float.MaxValue; + if (_driver == null) + return; + // Hold the stick down for 1 s (1.2 m/s → capped at 0.6 m here), hold, then back up. + float c = _t < 0.5f ? 0f : _t < 3.5f ? Mathf.Min(0.6f, (_t - 0.5f) * 1.2f) : Mathf.Max(0f, 0.6f - (_t - 3.5f) * 1.2f); + _driver.StickCrouch = c; + if (_t > 2.5f && _t < 3.4f && _ragdoll.Built) + { + _stickEyeLow = _ragdoll.Eye.Y; + _stickOriginLow = _origin.Origin.Y; + _stickEyeErr = Mathf.Max(_stickEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin)); + } + } + + private bool FinishStickCrouch() + { + GD.Print($"RAGDOLL: stick crouch 0.6 m: eye y {_stickEyeLow:F3} (standing {EyeHeight}), origin y {_stickOriginLow:F3}, eye vs camera {_stickEyeErr:F3}; back up: eye y {_ragdoll.Eye.Y:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}"); + bool ok = _finite && NewSnaps == 0 && Mathf.Abs(_stickEyeLow - (EyeHeight - 0.6f)) < 0.1f && Mathf.Abs(_stickOriginLow + 0.6f) < 0.08f + && _stickEyeErr < 0.06f && Mathf.Abs(_ragdoll.Eye.Y - EyeHeight) < 0.06f && Mathf.Abs(_origin.Origin.Y) < 0.06f; + _stickEyeErr = 0f; + return ok; } private Vector3 _crouchPelvis; diff --git a/scripts/TrueBody.cs b/scripts/TrueBody.cs index e5d8c94..3f8e03e 100644 --- a/scripts/TrueBody.cs +++ b/scripts/TrueBody.cs @@ -14,6 +14,7 @@ public sealed class TrueBodySolver 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°) @@ -79,16 +80,19 @@ public sealed class TrueBodySolver 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; - float hmdYaw = Mathf.Atan2(-headFwd.X, -headFwd.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; - Vector3 flatFwd = new Vector3(headFwd.X, 0f, headFwd.Z); - if (toHands.LengthSquared() > 1e-4f && flatFwd.LengthSquared() > 1e-6f) + if (toHands.LengthSquared() > 1e-4f) { - float handsYaw = Mathf.Atan2(-toHands.X, -toHands.Z); - float forwardness = Mathf.Clamp(toHands.Dot(flatFwd.Normalized()) / YawForwardness, 0f, 1f); + float handsYaw = BodyMath.YawOf(toHands); + float forwardness = Mathf.Clamp(toHands.Dot(flatFwd) / YawForwardness, 0f, 1f); targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight); } if (!Initialized) @@ -102,7 +106,8 @@ public sealed class TrueBodySolver 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))); + // 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); diff --git a/scripts/WorldPanel.cs b/scripts/WorldPanel.cs index a25dd2e..84a937d 100644 --- a/scripts/WorldPanel.cs +++ b/scripts/WorldPanel.cs @@ -1,7 +1,7 @@ using Godot; // The in-VR menu: a MenuScreen (Godot Control UI in a SubViewport on a thin 0.36x0.24 m -// panel) showing MainMenu. This node is the XR glue: summon/dismiss with ax_button (A/X), +// panel) showing MainMenu. This node is the XR glue: summon/dismiss with the LEFT ax_button (X; right A is jump), // grip-drag (direct or along the laser), re-orient toward the head, feed both controllers' // pointer rays into the screen, click sound + haptics. PARENTED UNDER XROrigin3D (fixed in the // player's room; game locomotion carries it). @@ -114,8 +114,7 @@ public partial class WorldPanel : Node3D public override void _Process(double delta) { - PollSummon(_left, 0); - PollSummon(_right, 1); + PollSummon(_left, 0); // left X only: right A is jump if (Visible) { UpdateDrag(_left, 0);