VR-Mess/scripts/PhysicsArms.cs

using Godot;

// Body-physics Stage 2: physical arm segments. Upper arm + forearm per side are chase-driven
// RigidBody3Ds (no joints — Box3D has no usable 3-DOF shoulder/wrist joint, and AGENTS.md
// forbids joints between tracked bodies). Each segment PD-chases its analytic 2-bone IK pose,
// solved every physics tick from the PHYSICAL chest shoulders to the PHYSICAL hand bodies.
// Hierarchical budgets (Nimso's muscle model): hand 90 N < forearm < upper arm < chest 2400 N.
// The arm IK + reach budget (torso lean on overreach) live here; BodyRig only draws.
public partial class PhysicsArms : Node
{
    private const float BodyMassStat = 60f;
    private const float ForearmMass = 1.2f;
    private const float UpperArmMass = 2.0f;
    private const float ForearmForcePerKg = 3f;   // 180 N at 60 kg
    private const float UpperArmForcePerKg = 6f;  // 360 N at 60 kg
    private const float SegmentMaxSpin = 40f;
    private const float ForearmMaxAlpha = 1500f;
    private const float UpperArmMaxAlpha = 1000f;
    private const float SegmentInset = 0.04f;
    public const float SnapDistance = 1f;

    private const float ShoulderRise = 0.25f;
    private const float ReachBudgetMax = 0.25f;
    private const float ReachVerticalMin = -0.05f;
    private const float ReachVerticalMax = 0.12f;
    private const float LeanReachFactor = 0.95f;
    private const float LeanSmoothRate = 6f;
    private const float WristOffset = 0.06f;
    private const float SoftIKFactor = 0.98f;
    private const float PoleHandWeight = 0.75f;
    private const float PoleSmoothRate = 12f;
    private const float ElbowRiseAllowance = 0.05f;

    public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg;
    public static float UpperArmMaxForce => BodyMassStat * UpperArmForcePerKg;

    // Injected by headless spikes; resolved from the main scene otherwise.
    public Node3D HeadSource;
    public PhysicsTorso Torso;
    public RigidBody3D HandLeft;
    public RigidBody3D HandRight;

    public bool Calibrated { get; private set; }
    public float UpperLen { get; private set; }
    public float ForeLen { get; private set; }
    public float ShoulderHalfWidth { get; private set; }

    private readonly RigidBody3D[] _upper = new RigidBody3D[2];
    private readonly RigidBody3D[] _fore = new RigidBody3D[2];
    private readonly Vector3[] _shoulder = new Vector3[2];
    private readonly Vector3[] _elbowTarget = new Vector3[2];
    private readonly Vector3[] _wristTarget = new Vector3[2];
    private readonly Vector3[] _lastPole = new Vector3[2];
    private readonly RigidBody3D[] _held = new RigidBody3D[2];
    private Vector3 _torsoOffset;
    private Basis _torsoBasis = Basis.Identity;

    public RigidBody3D UpperArm(int side) => _upper[side];
    public RigidBody3D Forearm(int side) => _fore[side];
    public Vector3 Shoulder(int side) => _shoulder[side];
    public Vector3 ElbowTarget(int side) => _elbowTarget[side];
    public Vector3 WristTarget(int side) => _wristTarget[side];

    // Physical elbow: where the two segment bodies meet (mean of their joint-side ends).
    public Vector3 Elbow(int side) =>
        (SegmentEnd(_upper[side], UpperLen, 1f) + SegmentEnd(_fore[side], ForeLen, -1f)) * 0.5f;

    public Vector3 Wrist(int side) => SegmentEnd(_fore[side], ForeLen, 1f);

    public override void _Ready()
    {
        if (HeadSource == null)
        {
            HeadSource = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
            Torso = GetNode<PhysicsTorso>("../PhysicsTorso");
            HandLeft = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/LeftHand"));
            HandRight = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/RightHand"));
        }
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        if (!Calibrated)
        {
            float headHeight = HeadSource.GlobalPosition.Y;
            if (headHeight <= 0.5f || Torso?.Body == null)
                return;
            Calibrate(headHeight);
        }

        _torsoBasis = Torso.Body.GlobalTransform.Basis;
        Vector3 topCenter = Torso.Body.GlobalPosition + Vector3.Up * ShoulderRise;
        _shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth;
        _shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth;

        SolveArm(0, -_torsoBasis.X, HandLeft, dt);
        SolveArm(1, _torsoBasis.X, HandRight, dt);
        UpdateLean(dt);

        for (int side = 0; side < 2; side++)
        {
            Drive(_upper[side], SegmentTarget(_shoulder[side], _elbowTarget[side], _upper[side]), UpperArmMaxForce, UpperArmMaxAlpha, dt);
            Drive(_fore[side], SegmentTarget(_elbowTarget[side], _wristTarget[side], _fore[side]), ForearmMaxForce, ForearmMaxAlpha, dt);
        }
    }

    private void Calibrate(float headHeight)
    {
        Calibrated = true;
        float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f);
        UpperLen = 0.5f * armTotal;
        ForeLen = 0.5f * armTotal;
        ShoulderHalfWidth = 0.16f + headHeight * 0.02f;

        for (int side = 0; side < 2; side++)
        {
            string name = side == 0 ? "Left" : "Right";
            _upper[side] = MakeSegment($"{name}UpperArmBody", UpperArmMass, new Vector3(0.07f, UpperLen - SegmentInset, 0.07f));
            _fore[side] = MakeSegment($"{name}ForearmBody", ForearmMass, new Vector3(0.06f, ForeLen - SegmentInset, 0.06f));
            SetHeldExceptions(side, _held[side], true);
        }
        GD.Print($"PhysicsArms: proportions calibrated from head height {headHeight:F2} m " +
                 $"(arm total {armTotal:F2} m, upper {UpperLen:F2} m, fore {ForeLen:F2} m, shoulder half-width {ShoulderHalfWidth:F2} m); " +
                 $"budgets fore {ForearmMaxForce:F0} N, upper {UpperArmMaxForce:F0} N");
    }

    private RigidBody3D MakeSegment(string name, float mass, Vector3 size)
    {
        RigidBody3D body = new()
        {
            Name = name,
            Mass = mass,
            GravityScale = 0f,
            ContinuousCd = true,
            CollisionLayer = 0b10000,  // layer 5 = player arms
            CollisionMask = 0b1,       // world + props only; own chest/hands/arms excluded
        };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        AddChild(body);
        // Spawn somewhere finite; the first Drive snaps it onto its target (> SnapDistance).
        body.GlobalPosition = Torso.Body.GlobalPosition + Vector3.Down * 100f;
        return body;
    }

    private static void Drive(RigidBody3D body, Transform3D target, float maxForce, float maxAlpha, float dt)
    {
        if (BodyMath.ResetIfNonFinite(body, target, "PhysicsArms"))
            return;
        if (body.GlobalPosition.DistanceTo(target.Origin) > SnapDistance)
        {
            body.GlobalTransform = target;
            body.LinearVelocity = Vector3.Zero;
            body.AngularVelocity = Vector3.Zero;
            return;
        }

        Vector3 desiredVelocity = (target.Origin - body.GlobalPosition) / dt;
        Vector3 force = ((desiredVelocity - body.LinearVelocity) / dt * body.Mass).LimitLength(maxForce);
        body.LinearVelocity += force / body.Mass * dt;

        Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(),
            body.GlobalTransform.Basis.GetRotationQuaternion());
        Vector3 spinDesired = (rotErr / dt).LimitLength(SegmentMaxSpin);
        body.AngularVelocity += (spinDesired - body.AngularVelocity).LimitLength(maxAlpha * dt);
    }

    // Segment pose from joint A to joint B: local +Y runs A→B; twist follows the body's current
    // X axis so the long-axis roll never flips (a box arm has no meaningful roll).
    private static Transform3D SegmentTarget(Vector3 a, Vector3 b, RigidBody3D body)
    {
        Vector3 y = b - a;
        if (y.LengthSquared() < 1e-8f)
            y = Vector3.Down;
        y = y.Normalized();
        Vector3 x = body.GlobalTransform.Basis.X;
        x -= y * x.Dot(y);
        if (x.LengthSquared() < 1e-6f)
        {
            x = y.Cross(Vector3.Forward);
            if (x.LengthSquared() < 1e-6f)
                x = y.Cross(Vector3.Right);
        }
        x = x.Normalized();
        return new Transform3D(new Basis(x, y, x.Cross(y)), (a + b) * 0.5f);
    }

    private static Vector3 SegmentEnd(RigidBody3D body, float length, float sign) =>
        body.GlobalPosition + body.GlobalTransform.Basis.Y * (sign * length * 0.5f);

    // A prop held by this side's hand must not collide with this side's arm: the forearm
    // naturally lies along a held object, and 180 N of forearm shoving a prop welded to a 90 N
    // hand is a fight the hand loses. Called by PropGrabber on grab/release.
    public void SetHeld(int side, RigidBody3D prop)
    {
        if (_held[side] == prop)
            return;
        SetHeldExceptions(side, _held[side], false);
        _held[side] = prop;
        SetHeldExceptions(side, prop, true);
    }

    private void SetHeldExceptions(int side, RigidBody3D prop, bool add)
    {
        if (prop == null || !IsInstanceValid(prop) || _upper[side] == null)
            return;
        foreach (RigidBody3D segment in new[] { _upper[side], _fore[side] })
        {
            if (add)
                segment.AddCollisionExceptionWith(prop);
            else
                segment.RemoveCollisionExceptionWith(prop);
        }
    }

    // Torso lean toward overreaching hands, computed OPEN-LOOP from the head-derived nominal
    // chest pose — never from the body's own shoulders (that loop made the chest hunt:
    // lean moves shoulders → overreach changes → lean changes). Exact two-arm combination:
    // the bigger excess first, then whatever the other arm still lacks after that shift.
    private void UpdateLean(float dt)
    {
        Basis basis = Torso.TargetBasis;
        Vector3 top = Torso.NominalTarget + Vector3.Up * ShoulderRise;
        Vector3 shoulderL = top - basis.X * ShoulderHalfWidth;
        Vector3 shoulderR = top + basis.X * ShoulderHalfWidth;
        Vector3 excessL = Excess(shoulderL, HandLeft);
        Vector3 excessR = Excess(shoulderR, HandRight);
        Vector3 lean = excessL.LengthSquared() >= excessR.LengthSquared()
            ? excessL + Excess(shoulderR + excessL, HandRight)
            : excessR + Excess(shoulderL + excessR, HandLeft);

        Vector3 horizontal = new Vector3(lean.X, 0f, lean.Z).LimitLength(ReachBudgetMax);
        lean = new Vector3(horizontal.X, Mathf.Clamp(lean.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z);
        _torsoOffset = _torsoOffset.Lerp(lean, 1f - Mathf.Exp(-dt * LeanSmoothRate));
        Torso.ReachOffset = _torsoOffset;
    }

    private Vector3 Excess(Vector3 shoulder, RigidBody3D hand)
    {
        if (hand == null)
            return Vector3.Zero;
        Vector3 toWrist = WristPoint(hand) - shoulder;
        float dist = toWrist.Length();
        float limit = (UpperLen + ForeLen) * LeanReachFactor;
        return dist > limit ? toWrist * ((dist - limit) / dist) : Vector3.Zero;
    }

    private static Vector3 WristPoint(RigidBody3D hand) =>
        hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset);

    // Analytic 2-bone IK: soft-IK clamp, then pick the elbow on its circle. The elbow
    // direction is mostly HAND-DRIVEN — the forearm continues behind the wrist along the
    // hand's (aim pose) forward axis, which is where a real forearm is; that gives outward
    // elbows for wraps/hugs and keeps the forearm out of held objects. A torso-frame
    // "down/back/out" pole fills in where the hand axis says nothing (arm pointing straight
    // along the hand). The pole is smoothed by ROTATING it at a bounded rate around the arm
    // axis, so it can never pass through zero and flip.
    private void SolveArm(int index, Vector3 side, RigidBody3D hand, float delta)
    {
        Vector3 shoulder = _shoulder[index];
        Vector3 rawTarget = hand != null ? WristPoint(hand) : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f);

        float reach = UpperLen + ForeLen;
        Vector3 shoulderToTarget = rawTarget - shoulder;
        if (shoulderToTarget.LengthSquared() < 0.0001f)
            shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f);

        float rawDist = shoulderToTarget.Length();
        float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor);
        Vector3 dir = shoulderToTarget / rawDist;
        Vector3 wrist = shoulder + dir * d;

        // Elbow circle: center on the arm axis, radius from the law of cosines.
        float along = (UpperLen * UpperLen - ForeLen * ForeLen + d * d) / (2f * d);
        float radius = Mathf.Sqrt(Mathf.Max(UpperLen * UpperLen - along * along, 0f));
        Vector3 center = shoulder + dir * along;

        Vector3 natural = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
        natural -= dir * natural.Dot(dir);
        Vector3 pole = natural.LengthSquared() > 1e-8f ? natural.Normalized() * (1f - PoleHandWeight) : Vector3.Zero;
        if (hand != null)
        {
            Vector3 handForward = (hand.GlobalTransform.Basis * Vector3.Forward).Normalized();
            Vector3 handElbow = wrist - handForward * ForeLen - center;
            handElbow -= dir * handElbow.Dot(dir);
            float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f);
            if (handElbow.LengthSquared() > 1e-8f)
                pole += handElbow.Normalized() * (PoleHandWeight * confidence);
        }

        Vector3 last = _lastPole[index];
        last -= dir * last.Dot(dir);
        if (last.LengthSquared() < 1e-8f)
            last = pole.LengthSquared() > 1e-8f ? pole : natural;
        last = last.LengthSquared() > 1e-8f ? last.Normalized() : Vector3.Down;
        if (pole.LengthSquared() > 1e-8f)
        {
            pole = pole.Normalized();
            float angle = last.AngleTo(pole);
            if (angle > 1e-5f)
            {
                Vector3 axis = last.Cross(pole);
                axis = axis.LengthSquared() > 1e-10f ? axis.Normalized() : dir;
                last = last.Rotated(axis, angle * (1f - Mathf.Exp(-delta * PoleSmoothRate)));
            }
        }
        _lastPole[index] = last;

        Vector3 elbow = center + CapElbowHeight(last, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius;
        _elbowTarget[index] = elbow;
        _wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen;
    }

    // A real wrist flexes, so following the hand axis literally can throw the elbow above
    // the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down
    // its circle, preserving its sideways heading.
    private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax)
    {
        if (center.Y + pole.Y * radius <= yMax)
            return pole;
        Vector3 vertical = Vector3.Up - dir * dir.Y;
        if (vertical.LengthSquared() < 1e-6f)
            return pole;  // arm vertical: the circle is horizontal, height can't change
        vertical = vertical.Normalized();
        Vector3 lateral = dir.Cross(vertical);
        float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f);
        float sin = Mathf.Sqrt(1f - cosMax * cosMax);
        float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f;
        return (vertical * cosMax + lateral * (sin * sideSign)).Normalized();
    }
}