VR-Mess/scripts/PlayerRagdoll.cs

using Godot;
using System.Collections.Generic;

// Body step 2 — the player IS a muscle-driven ragdoll (Nimso's Project TX, research/nimso-project-tx.md
// §2): every bone drives ITSELF toward the True Body pose through torque-capped joint motors
// (never world-pinned), the hands are additionally pulled to the controllers by a force-capped
// guide that reacts into the chest, and "ground control" holds the pelvis up and carries the body
// after the head. Input-agnostic: a driver (PlayerRig in-game, RagdollSpike headless) solves the
// TrueBodySolver, then calls Step(); the camera stays 1:1 and the driver moves its play space by
// OriginCorrection() when the body is blocked (push-back) or stands on higher/lower ground.
//
// Bodies (boxes, origin = centre of mass): pelvis (+ the not-yet-physical legs' mass), chest,
// head (incl. neck), and per arm upper arm, forearm, hand. No light intermediate links: a 0.6 kg
// neck or 0.8 kg clavicle squeezed between two strong motors oscillates under Box3D's
// one-iteration-per-substep solver (measured: RagdollSpike, 2026-10-02).
// Joints (fork Generic6DOFJoint3D mapping, research/godot-box3d-6dof.md):
//   spine, neck, wrist = BALL (cone/twist limits), spine/neck with muscle motors;
//   shoulder = DRIVE chest→upper arm: linear motor holds the humeral head on the (moving) girdle
//     point, angular motor = the shoulder muscle (no cone limit — Box3D's motor joint has none);
//   elbow = HingeJoint3D (0..150°) with a muscle motor;
//   hand guide = DRIVE chest→hand; balance = DRIVE world anchor→pelvis (angular only).
// 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
	// 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<RigidBody3D> _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<RigidBody3D, Transform3D> _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, _sinceBraced = float.MaxValue;
	private PhysicsShapeQueryParameters3D _legsQuery;

	private bool LegsSpaceFree(Vector3 center, Basis basis)
	{
		_legsQuery ??= new PhysicsShapeQueryParameters3D { Shape = Legs.GetChild<CollisionShape3D>(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<RigidBody3D> 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(ShoulderFrame(dir), shoulder), GirdleForce, ShoulderTorque,
				j => ShoulderRange(j, sign));
			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<Generic6DofJoint3D.Param, float>[] { 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 };
	}

	// Shoulder joint frame: X = the humerus as built (the twist axis), Y = "up" square to it, Z = X × Y.
	// The range below is measured from this build pose (relative rotation identity).
	private static Basis ShoulderFrame(Vector3 bone)
	{
		Vector3 y = (Vector3.Up - bone * bone.Y).Normalized();
		return new Basis(bone, y, bone.Cross(y));
	}

	// Anatomical range (fork: drive-mode angular limits): swing bounds about joint Y/Z, an asymmetric
	// ellipse fitted so hanging, 45° extension, overhead (+10° behind), lateral 30° behind, a 45°-across
	// hug and a hand behind the back fit with ≥ 2.5° to spare, while straight back, 80° extension,
	// behind-the-head and medial-horizontal stay ≥ 8° out. The left arm mirrors Y (sign = −1).
	public static readonly float ShoulderSwingBack = Mathf.DegToRad(90f), ShoulderSwingAcross = Mathf.DegToRad(100f);
	public static readonly float ShoulderSwingDown = Mathf.DegToRad(85f), ShoulderSwingUp = Mathf.DegToRad(155f);
	public static readonly float ShoulderTwist = Mathf.DegToRad(80f);

	private static void ShoulderRange(Generic6DofJoint3D j, float sign)
	{
		j.SetFlagX(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
		j.SetFlagY(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
		j.SetFlagZ(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
		j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -ShoulderTwist);
		j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, ShoulderTwist);
		j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, sign > 0f ? -ShoulderSwingBack : -ShoulderSwingAcross);
		j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, sign > 0f ? ShoulderSwingAcross : ShoulderSwingBack);
		j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, -ShoulderSwingDown);
		j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, ShoulderSwingUp);
	}

	private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque,
		System.Action<Generic6DofJoint3D> configure = null)
	{
		Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
		foreach (var flag in new System.Action<Generic6DofJoint3D.Flag, bool>[] { 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<Generic6DofJoint3D.Param, float>[] { j.SetParamX, j.SetParamY, j.SetParamZ })
		{
			param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force);
			param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
		}
		configure?.Invoke(j);  // before AddChild: limits/flags rebuild the server joint
		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);
		}
		_sinceBraced = Braced ? 0f : _sinceBraced + dt;
		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 something solid enough to push yourself up on (a ledge, a wall,
	// a grabbed bar, a heavy crate) — not the floor: hands on the floor while crawling don't lift the
	// body. Nimso: physics objects are climbed "the same" as the world. A light prop just gets pushed,
	// and something you're carrying isn't support.
	public const float BraceMinMass = 15f;
	public const float ClamberMemory = 0.6f;            // s a hand press still counts after the hands leave

	private static bool IsBraced(Arm a)
	{
		if (a.Held is StaticBody3D || a.Held is RigidBody3D { Freeze: true })
			return true;
		foreach (Node3D n in a.Hand.GetCollidingBodies())
		{
			if (n == a.Held || n.IsInGroup(PropGrabber.NoGrabGroup))
				continue;
			if (n is StaticBody3D || n is RigidBody3D { Freeze: true } || n is RigidBody3D { Mass: >= BraceMinMass })
				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;
		// Hands braced while the hips are held back by an obstacle = getting a knee up onto it (a
		// thigh-high crate or ledge the straight arms can't press down on while standing).
		bool kneeUp = Braced && HeldBack != Vector3.Zero;
		bool wantTuck = deep || _lifted || kneeUp || (!_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<CollisionShape3D>(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;
		bool wasGrounded = Grounded;
		Grounded = false;
		_groundBody = null;
		if (ProbeGround(pos, hipAboveFloor + GroundProbeExtra + 0.5f, wasGrounded, out Vector3 point, out Vector3 normal, out GodotObject groundObject, out bool underHips))
		{
			_groundBody = groundObject as RigidBody3D;
			_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 (Nimso: higher ground "doesn't move my body up … until I push on it with enough
				// pressure, at which point I actually clamber over"): ground more than a step above the
				// feet is stood on once the hips are over it AND the body is pressing onto it — hands
				// braced on anything solid (ledge, crate), or already airborne
				// (landing, hanging). Otherwise it's an obstacle the legs collider holds us back from.
				// No "hips well above it" requirement: that made a ledge need a full press-up.
				bool step = point.Y <= _footY + MaxStepHeight;
				// The press lasts a moment after the hands leave (you push up, then move over it), and legs
				// tucked up with the hips over it are already on top of it.
				bool pressing = !wasGrounded || Braced || _lifted || _sinceBraced < ClamberMemory || !_legsSolid;
				bool reachable = step || (underHips && pressing);
				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);
			// Legs can't hold the hips higher than straight legs (head above the calibrated height —
			// tiptoes, a reach up — would otherwise float the body off the ground).
			goal.Y = Mathf.Min(goal.Y, _footY + _standingHip + JumpExtension);
			_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;

	// Ground under the body (Nimso: "ground detection uses the objects underneath the body"), not
	// under one point: a sphere the width of the stance is cast down from the hips, plus the
	// centre ray. One ray lost the ground as soon as the hips swayed off a plank or past a ledge lip.
	// The sphere's contact can be a corner, whose normal is useless — the surface normal comes from a
	// short ray just inside the contact. Same-level ground (within a step) from either probe is
	// stood on; higher ground only from the centre ray (the hips must really be over it), and it's
	// reported via underHips for the pressure rule.
	public const float FootprintRadius = 0.12f;

	private bool ProbeGround(Vector3 pos, float length, bool wasGrounded, out Vector3 point, out Vector3 normal, out GodotObject collider, out bool underHips)
	{
		PhysicsDirectSpaceState3D space = GetWorld3D().DirectSpaceState;
		point = normal = Vector3.Zero;
		collider = null;
		underHips = false;
		Godot.Collections.Dictionary ray = space.IntersectRay(PhysicsRayQueryParameters3D.Create(pos, pos + Vector3.Down * length, WorldMask));
		bool haveRay = ray.Count > 0;
		if (haveRay)
		{
			point = (Vector3)ray["position"];
			normal = (Vector3)ray["normal"];
			collider = GodotObject.InstanceFromId((ulong)ray["collider_id"]);  // Box3D: "collider" is null
			underHips = true;
		}
		_footprintQuery ??= new PhysicsShapeQueryParameters3D { Shape = new SphereShape3D { Radius = FootprintRadius }, CollisionMask = WorldMask };
		_footprintQuery.Transform = new Transform3D(Basis.Identity, pos);
		_footprintQuery.Motion = Vector3.Down * length;
		Godot.Collections.Dictionary rest = space.GetRestInfo(_footprintQuery);
		if (rest.Count == 0)
			return haveRay;
		Vector3 contact = (Vector3)rest["point"];
		if (contact.Y >= pos.Y - 0.05f)
			return haveRay;   // touching a wall beside the hips, not ground
		Vector3 inward = new(contact.X - pos.X, 0f, contact.Z - pos.Z);
		inward = inward.LengthSquared() > 1e-6f ? inward.Normalized() * 0.02f : Vector3.Zero;
		Vector3 probe = contact + inward;
		Godot.Collections.Dictionary face = space.IntersectRay(PhysicsRayQueryParameters3D.Create(
			new Vector3(probe.X, contact.Y + 0.05f, probe.Z), new Vector3(probe.X, contact.Y - 0.05f, probe.Z), WorldMask));
		if (face.Count == 0)
			return haveRay;
		Vector3 sPoint = (Vector3)face["position"], sNormal = (Vector3)face["normal"];
		if (sNormal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees)))
			return haveRay;
		// The sphere wins where the centre ray found nothing, nothing walkable, or only lower ground
		// within the stance (a plank's edge under the hips) — but never higher ground than a step.
		bool sameLevel = sPoint.Y <= _footY + MaxStepHeight || !wasGrounded;
		bool better = !haveRay || normal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees)) || sPoint.Y > point.Y + 0.01f;
		if (sameLevel && better)
		{
			point = sPoint;
			normal = sNormal;
			collider = GodotObject.InstanceFromId((ulong)face["collider_id"]);
			underHips = true;   // under the footprint
		}
		return true;
	}

	private PhysicsShapeQueryParameters3D _footprintQuery;

	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();
	}

	// What each hand holds (bracing, climbing). Only the hand itself is excluded from the held object
	// (by the grab weld).
	public void SetHeld(int side, PhysicsBody3D prop)
	{
		if (!Built)
			return;
		// The arm keeps colliding with what the hand holds (owner: real arms don't pass through things).
		// The old per-object exception switched the arm off against a whole crag rock while
		// gripping one of its holds — arms "randomly" phased through the world mid-climb.
		_arms[side].Held = prop;
	}
}