VR-Mess/scripts/BodyRig.cs

using Godot;

public partial class BodyRig : Node3D
{
    private const float HeadBackOffset = 0.06f;
    private const float HeadSize = 0.15f;
    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 ReachPushSpeed = 0.8f;
    private const float ReachDecaySpeed = 0.4f;
    private const float ReachDecayFactor = 0.92f;
    private const float WristOffset = 0.06f;
    private const float SoftIKFactor = 0.98f;
    private const float PoleHandWeight = 0.25f;
    private const float PoleSmoothRate = 10f;

    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;
    private PhysicsTorso _torso;

    private MeshInstance3D _headMesh;
    private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2];
    private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2];

    private bool _calibrated;
    private float _upperLen;
    private float _foreLen;
    private float _shoulderHalfWidth;
    private Vector3 _torsoOffset = Vector3.Zero;
    private Basis _yawBasis = Basis.Identity;
    private Basis _torsoBasis = Basis.Identity;
    private readonly Vector3[] _lastPole = new Vector3[2];

    public override void _Ready()
    {
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _torso = GetNode<PhysicsTorso>("../PhysicsTorso");

        _headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f));
        _upperArmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
        _upperArmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
        _forearmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
        _forearmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));

        SetRigVisible(false);
    }

    private MeshInstance3D MakeMesh(Mesh mesh, Color color)
    {
        MeshInstance3D node = new() { Mesh = mesh };
        node.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f });
        AddChild(node);
        return node;
    }

    private void SetRigVisible(bool visible)
    {
        _headMesh.Visible = visible;
        foreach (MeshInstance3D m in _upperArmMeshes)
            m.Visible = visible;
        foreach (MeshInstance3D m in _forearmMeshes)
            m.Visible = visible;
    }

    public override void _Process(double delta)
    {
        UpdateYawBasis();
        float dt = (float)delta;

        if (!_calibrated)
        {
            float headHeight = _camera.GlobalPosition.Y;
            if (headHeight <= 0.5f)
                return;
            Calibrate(headHeight);
        }
        if (_torso.Body == null)
            return;

        Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
        _headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos);

        _torsoBasis = _torso.Body.GlobalTransform.Basis;
        Vector3 torsoCenter = _torso.Body.GlobalPosition;

        Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise;
        Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth;
        Vector3 rightShoulder = topCenter + _torsoBasis.X * _shoulderHalfWidth;

        Vector3 push = SolveArm(0, leftShoulder, -_torsoBasis.X, PhysicsHands.GetBody(_left), _upperArmMeshes[0], _forearmMeshes[0], dt, out bool decayLeft);
        push += SolveArm(1, rightShoulder, _torsoBasis.X, PhysicsHands.GetBody(_right), _upperArmMeshes[1], _forearmMeshes[1], dt, out bool decayRight);

        if (push != Vector3.Zero)
        {
            _torsoOffset += push * (dt * ReachPushSpeed);
        }
        else if (decayLeft || decayRight)
        {
            _torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt);
        }

        Vector3 horizontal = new(_torsoOffset.X, 0f, _torsoOffset.Z);
        horizontal = horizontal.LimitLength(ReachBudgetMax);
        _torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z);
        _torso.ReachOffset = _torsoOffset;
    }

    private void UpdateYawBasis()
    {
        Vector3 fwd = -_camera.GlobalTransform.Basis.Z;
        fwd.Y = 0f;
        if (fwd.LengthSquared() < 0.0001f)
            return;
        _yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up);
    }

    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;

        ((BoxMesh)_upperArmMeshes[0].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
        ((BoxMesh)_upperArmMeshes[1].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
        ((BoxMesh)_forearmMeshes[0].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);
        ((BoxMesh)_forearmMeshes[1].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);

        SetRigVisible(true);
        GD.Print($"BodyRig: 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)");
    }

    private Vector3 SolveArm(int index, Vector3 shoulder, Vector3 side, RigidBody3D body, MeshInstance3D upperMesh, MeshInstance3D foreMesh, float delta, out bool decay)
    {
        Vector3 rawTarget = body != null
            ? body.GlobalPosition + body.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset)
            : 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 clampedTarget = shoulder + dir * d;

        Vector3 push = Vector3.Zero;
        decay = false;
        if (rawDist > reach)
        {
            push = (rawTarget - clampedTarget).Normalized();
        }
        else if (rawDist < reach * ReachDecayFactor)
        {
            decay = true;
        }

        float cosA = (_upperLen * _upperLen + d * d - _foreLen * _foreLen) / (2f * _upperLen * d);
        float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f));

        Vector3 handForward = body != null ? body.GlobalTransform.Basis * Vector3.Forward : Vector3.Zero;
        Vector3 pole = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
        pole -= dir * pole.Dot(dir);
        pole -= handForward * PoleHandWeight;
        Vector3 poleDir;
        if (pole.LengthSquared() < 0.0001f)
            poleDir = _lastPole[index] == Vector3.Zero ? Vector3.Down : _lastPole[index];
        else
        {
            poleDir = _lastPole[index].Lerp(pole, 1f - Mathf.Exp(-delta * PoleSmoothRate));
            if (poleDir.LengthSquared() < 0.0001f)
                poleDir = _lastPole[index];
            poleDir = poleDir.Normalized();
            _lastPole[index] = poleDir;
        }

        Vector3 axis = dir.Cross(poleDir);
        if (axis.LengthSquared() < 0.0001f)
            axis = dir.Cross(Vector3.Up);
        if (axis.LengthSquared() < 0.0001f)
            axis = Vector3.Right;
        axis = axis.Normalized();

        Vector3 elbowPos = shoulder + dir.Rotated(axis, angle) * _upperLen;
        Vector3 foreEnd = elbowPos + (clampedTarget - elbowPos).Normalized() * _foreLen;

        PlaceSegment(upperMesh, shoulder, elbowPos);
        PlaceSegment(foreMesh, elbowPos, foreEnd);

        return push;
    }

    private static void PlaceSegment(MeshInstance3D mesh, Vector3 from, Vector3 to)
    {
        Vector3 segment = to - from;
        if (segment.LengthSquared() < 0.000001f)
            return;
        Vector3 y = segment.Normalized();
        Vector3 x = Vector3.Up.Cross(y);
        if (x.LengthSquared() < 0.0001f)
            x = Vector3.Right.Cross(y);
        x = x.Normalized();
        Vector3 z = x.Cross(y);
        mesh.GlobalTransform = new Transform3D(new Basis(x, y, z), (from + to) * 0.5f);
    }
}