VR-Mess/scripts/PlayerRig.cs

using Godot;

// In-game driver of the ragdoll player (body step 2): XR input → True Body → PlayerRagdoll, plus
// 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);
//   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.
public partial class PlayerRig : Node
{
    public const float MoveSpeed = 2.2f;           // m/s at full stick (Boneworks-ish walk)
    public const float MoveAcceleration = 8f;      // m/s²
    public const float StickDeadzone = 0.2f;
    public static bool SmoothTurn = false;
    public const float SnapTurnDegrees = 30f;
    public const float SmoothTurnSpeed = 2.5f;     // rad/s
    public const float SnapTurnThreshold = 0.7f;
    public const float SnapTurnRearm = 0.4f;
    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;

    private static PlayerRig _instance;

    private XROrigin3D _origin;
    private XRCamera3D _camera;
    private XRController3D _left, _right;
    private TrueBody _trueBody;
    private PlayerRagdoll _ragdoll;
    private Transform3D _lastOrigin;
    private bool _haveLastOrigin;
    private Vector3 _moveVelocity;
    private bool _snapArmed = true;
    private readonly Vector3[] _ctrlPrev = new Vector3[2];
    private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
    private bool _rebuild = true;

    public PlayerRagdoll Ragdoll => _ragdoll;
    public static bool Spectator { get; private set; }

    // Spectator = no body (fly with the old grab-drag movement); leaving it rebuilds the body at
    // the current pose.
    public static void SetSpectator(bool on)
    {
        Spectator = on;
        AnchorMoveMode.Enabled = on;
        if (_instance == null)
            return;
        if (on)
            _instance._ragdoll.Clear();
        _instance._rebuild = true;
        GD.Print($"PlayerRig: spectator {(on ? "on" : "off")}");
    }

    public static RigidBody3D GetHandBody(XRController3D controller)
    {
        if (_instance == null || !_instance._ragdoll.Built)
            return null;
        return _instance._ragdoll.Hand(controller == _instance._left ? 0 : 1);
    }

    public static void SetHeld(XRController3D controller, PhysicsBody3D prop)
    {
        if (_instance != null)
            _instance._ragdoll.SetHeld(controller == _instance._left ? 0 : 1, prop);
    }

    public override void _Ready()
    {
        _instance = this;
        _origin = GetNode<XROrigin3D>("../XROrigin3D");
        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _trueBody = GetNodeOrNull<TrueBody>("../TrueBody");
        if (_trueBody != null)
        {
            _trueBody.ExternallySolved = true;
            TrueBody.ShowDebug = false;
        }
        // PlayerRig itself is excluded from physics interpolation (ghosts follow controllers per
        // frame); the body is simulated, so it interpolates.
        _ragdoll = new PlayerRagdoll { Name = "Ragdoll", PhysicsInterpolationMode = PhysicsInterpolationModeEnum.On };
        AddChild(_ragdoll);
        BodyProfile.Changed += _ => _rebuild = true;
        for (int i = 0; i < 2; i++)
        {
            _ghosts[i] = new MeshInstance3D
            {
                Mesh = new BoxMesh { Size = PlayerRagdoll.HandSize },
                Visible = false,
                CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
            };
            _ghosts[i].SetSurfaceOverrideMaterial(0, new StandardMaterial3D
            {
                AlbedoColor = new Color(1f, 1f, 1f, 0.3f),
                Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
                ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
            });
            AddChild(_ghosts[i]);
        }
    }

    private TrueBodySolver Solver => _trueBody != null ? _trueBody.Solver : _ownSolver;
    private readonly TrueBodySolver _ownSolver = new();

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        BodyProfile profile = BodyProfile.Current;
        if (profile == null || Spectator)
        {
            _haveLastOrigin = false;
            return;
        }

        // 1. Someone else moved the play space discretely: bring the body along.
        Transform3D origin = _origin.GlobalTransform;
        if (_haveLastOrigin && _ragdoll.Built && IsJump(_lastOrigin, origin))
            Carry(origin * _lastOrigin.AffineInverse());

        if (_ragdoll.Built && !_rebuild)
        {
            // 2. Stick locomotion, 3. reconciliation.
            Locomote(dt);
            Vector3 correction = _ragdoll.OriginCorrection(_camera.GlobalPosition, _origin.GlobalPosition.Y, dt);
            if (correction != Vector3.Zero)
                _origin.GlobalPosition += correction;
        }

        // 4. Pose + muscles.
        Transform3D head = _camera.GlobalTransform;
        Transform3D left = _left.GlobalTransform, right = _right.GlobalTransform;
        Solver.Solve(profile, head, left, right, _origin.GlobalPosition.Y, dt);
        if (_rebuild || !_ragdoll.Built)
        {
            _ragdoll.Build(profile, Solver, left, right);
            _rebuild = false;
            _ctrlPrev[0] = left.Origin;
            _ctrlPrev[1] = right.Origin;
        }
        Vector3 lv = (left.Origin - _ctrlPrev[0]) / dt, rv = (right.Origin - _ctrlPrev[1]) / dt;
        _ctrlPrev[0] = left.Origin;
        _ctrlPrev[1] = right.Origin;
        _ragdoll.Step(Solver, left, right, lv, rv, _origin.GlobalPosition.Y, dt);
        _lastOrigin = _origin.GlobalTransform;
        _haveLastOrigin = true;
    }

    public override void _Process(double delta)
    {
        for (int i = 0; i < 2; i++)
        {
            XRController3D c = i == 0 ? _left : _right;
            RigidBody3D hand = _ragdoll.Built ? _ragdoll.Hand(i) : null;
            bool lagging = hand != null && hand.GlobalPosition.DistanceTo(c.GlobalPosition) > GhostLagDistance;
            _ghosts[i].Visible = lagging;
            if (lagging)
                _ghosts[i].GlobalTransform = c.GlobalTransform;
        }
    }

    private static bool IsJump(Transform3D a, Transform3D b) =>
        a.Origin.DistanceTo(b.Origin) > JumpDistance ||
        BodyMath.RotationError(b.Basis.GetRotationQuaternion(), a.Basis.GetRotationQuaternion()).Length() > JumpAngle;

    private void Carry(Transform3D delta)
    {
        _ragdoll.TeleportBy(delta);
        Solver.Shift(delta);
        _ctrlPrev[0] = delta * _ctrlPrev[0];
        _ctrlPrev[1] = delta * _ctrlPrev[1];
    }

    private void Locomote(float dt)
    {
        // A stick whose hand points at the menu scrolls it instead.
        Vector2 move = WorldPanel.IsPointing(_left) ? Vector2.Zero : _left.GetVector2("primary");
        Vector2 turn = WorldPanel.IsPointing(_right) ? Vector2.Zero : _right.GetVector2("primary");

        // Move: head-relative on the ground plane, acceleration-limited. The body follows the
        // camera physically, so a wall stops it and the push-back stops the view.
        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 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);
        }
        if (!_ragdoll.Grounded)
            desired = _moveVelocity;  // no air control
        _moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt);
        if (_moveVelocity != Vector3.Zero)
            _origin.GlobalPosition += _moveVelocity * dt;

        // Turn about the head.
        float angle = 0f;
        if (SmoothTurn)
        {
            if (Mathf.Abs(turn.X) > StickDeadzone)
                angle = -turn.X * SmoothTurnSpeed * dt;
        }
        else if (_snapArmed && Mathf.Abs(turn.X) > SnapTurnThreshold)
        {
            angle = -Mathf.Sign(turn.X) * Mathf.DegToRad(SnapTurnDegrees);
            _snapArmed = false;
        }
        else if (Mathf.Abs(turn.X) < SnapTurnRearm)
            _snapArmed = true;
        if (angle != 0f)
        {
            Vector3 pivot = _camera.GlobalPosition;
            Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, angle), Vector3.Zero)
                * new Transform3D(Basis.Identity, -pivot);
            _origin.GlobalTransform = rot * _origin.GlobalTransform;
            if (!SmoothTurn)
                Carry(rot);
        }
    }
}