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),
//      right Y = crouch (down lowers, up raises — RagdollDriver.StickCrouch); right A = hold to charge, release to jump;
//   3. the ragdoll's OriginCorrection (push-back at walls, floor follows the ground, falls);
//   4. True Body solve from the corrected camera/controllers, then the ragdoll's muscles.
// Waits for the calibrated BodyProfile like everything else; rebuilds on recalibration.
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 StickCrouchSpeed = 1.2f;    // m/s at full stick
    public const float StickCrouchHeadroom = 0.5f; // the stick can lower the eyes to this height (a crawl)
    public const float JumpDistance = 0.5f;        // an origin move this big in one tick = teleport, not motion
    public const float JumpAngle = 0.1f;           // rad
    private const float GhostLagDistance = 0.1f;

    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 MeshInstance3D[] _ghosts = new MeshInstance3D[2];
    private bool _rebuild = true;
    private RagdollDriver _driver;
    // Render interpolation of the play space (smooth-camera bible, VR edition): physics moves the
    // origin every tick, but the body is DRAWN interpolated between the last two ticks. A camera
    // stepping with the raw origin while the body glides a tick behind looks like the body is in
    // two places at once when you move and look at it. So physics owns _physOrigin and every
    // frame the visible origin is the same interpolation of it the bodies get.
    private Transform3D _physOrigin, _prevPhysOrigin, _shownOrigin;
    private bool _interpolateOrigin;

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

    // 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;
        ProcessPriority = -100;          // place the interpolated origin before anything reads it this frame
        ProcessPhysicsPriority = -100;   // restore the physics origin before anything uses it this tick
        _origin = GetNode<XROrigin3D>("../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);
        _driver = new RagdollDriver { Solver = Solver, Ragdoll = _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;
            _interpolateOrigin = false;
            return;
        }
        RestorePhysicsOrigin();

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

        // 2. Stick locomotion.
        if (_ragdoll.Built && !_driver.RebuildRequested)
            Locomote(dt);

        // 3. Filtered tracking → play-space correction → True Body → muscles (shared with RagdollSpike).
        _driver.RebuildRequested |= _rebuild;
        _rebuild = false;
        Transform3D o = _origin.GlobalTransform;
        _driver.Tick(profile, ref o, _camera.Transform, _left.Transform, _right.Transform, _moveVelocity, dt);
        _origin.GlobalTransform = o;
        _lastOrigin = _origin.GlobalTransform;
        _haveLastOrigin = true;
        _prevPhysOrigin = _interpolateOrigin && !IsJump(_physOrigin, _lastOrigin) ? _physOrigin : _lastOrigin;
        _physOrigin = _shownOrigin = _lastOrigin;
        _interpolateOrigin = true;
    }

    // The origin on screen is interpolated; physics must run on the real one. Anyone else who moved
    // the origin since (teleport marker, menu) is respected as the new real one.
    private void RestorePhysicsOrigin()
    {
        if (!_interpolateOrigin)
            return;
        Transform3D current = _origin.GlobalTransform;
        if (current.IsEqualApprox(_shownOrigin))
            _origin.GlobalTransform = _physOrigin;
        else
            _physOrigin = _prevPhysOrigin = current;
    }

    public override void _Process(double delta)
    {
        if (_interpolateOrigin && !Spectator)
        {
            Transform3D current = _origin.GlobalTransform;
            if (!current.IsEqualApprox(_shownOrigin))
                _physOrigin = _prevPhysOrigin = current;  // moved by someone else: show it as is
            _shownOrigin = _prevPhysOrigin.InterpolateWith(_physOrigin, (float)Engine.GetPhysicsInterpolationFraction());
            _origin.GlobalTransform = _shownOrigin;
        }
        for (int i = 0; i < 2; i++)
        {
            XRController3D c = i == 0 ? _left : _right;
            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;
        }
    }

    // The body is held back (hips against a table, a wall the head clears): stop the stick from
    // carrying the view further that way.
    public static Vector3 HoldBack(Vector3 velocity, Vector3 heldBack)
    {
        float into = velocity.Dot(heldBack);
        return into > 0f ? velocity - heldBack * into : velocity;
    }

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

    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 = BodyMath.HeadFacing(_camera.GlobalTransform.Basis.Orthonormalized());
            if (fwd == Vector3.Zero)
                fwd = -Solver.YawBasis.Z;
            Vector3 rightDir = fwd.Cross(Vector3.Up);
            float mag = Mathf.Clamp((move.Length() - StickDeadzone) / (1f - StickDeadzone), 0f, 1f);
            desired = (fwd * move.Y + rightDir * move.X).Normalized() * (mag * MoveSpeed);
        }
        if (!_ragdoll.Grounded)
            desired = _moveVelocity;  // no air control
        _moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt);
        _moveVelocity = HoldBack(_moveVelocity, _ragdoll.HeldBack);
        if (_moveVelocity != Vector3.Zero)
            _origin.GlobalPosition += _moveVelocity * dt;

        // Jump: hold right A to charge (the legs bend), release to jump.
        _ragdoll.JumpHeld = _right.IsButtonPressed("ax_button");

        // Crouch: right stick held down lowers the body, up raises it back (the stick's dominant
        // axis decides between crouch and turn).
        bool vertical = Mathf.Abs(turn.Y) > Mathf.Abs(turn.X);
        if (vertical && Mathf.Abs(turn.Y) > StickDeadzone)
        {
            float max = Mathf.Max(0f, BodyProfile.Current.EyeHeight - StickCrouchHeadroom);
            _driver.StickCrouch = Mathf.Clamp(_driver.StickCrouch - turn.Y * StickCrouchSpeed * dt, 0f, max);
        }
        if (vertical)
            turn.X = 0f;

        // Turn about the head.
        float angle = 0f;
        if (SmoothTurn)
        {
            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);
        }
    }
}