VR-Mess/scripts/PhysicsHands.cs

using Godot;

public partial class PhysicsHands : Node
{
    private const float HandMass = 0.4f;
    public const float HandMaxForce = 90f;
    private const float HandMaxSpin = 120f;
    public const float SnapDistance = 2f;
    private const float GhostLagDistance = 0.1f;
    private const float StuckDistance = 0.2f;
    private const float StuckTime = 1.0f;
    private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);

    private static PhysicsHands _instance;

    private XRController3D _left;
    private XRController3D _right;
    private PhysicsArms _arms;
    private readonly RigidBody3D[] _bodies = new RigidBody3D[2];
    private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
    private readonly float[] _stuckTimer = new float[2];

    public override void _Ready()
    {
        _instance = this;
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _arms = GetNodeOrNull<PhysicsArms>("../PhysicsArms");
        _bodies[0] = MakeHand("LeftHandBody", new Color(0.9f, 0.3f, 0.35f));
        _bodies[1] = MakeHand("RightHandBody", new Color(0.3f, 0.5f, 0.9f));
        _ghosts[0] = MakeGhost();
        _ghosts[1] = MakeGhost();
        GD.Print("PhysicsHands: physical hand bodies spawned");
    }

    private MeshInstance3D MakeGhost()
    {
        MeshInstance3D ghost = new()
        {
            Mesh = new BoxMesh { Size = HandSize },
            Visible = false,
            CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
        };
        ghost.SetSurfaceOverrideMaterial(0, new StandardMaterial3D
        {
            AlbedoColor = new Color(1f, 1f, 1f, 0.3f),
            Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
            ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
        });
        AddChild(ghost);
        return ghost;
    }

    public static RigidBody3D GetBody(XRController3D controller)
    {
        if (_instance == null)
            return null;
        return _instance._left == controller ? _instance._bodies[0] : _instance._bodies[1];
    }

    private RigidBody3D MakeHand(string name, Color color)
    {
        RigidBody3D body = new()
        {
            Name = name,
            Mass = HandMass,
            GravityScale = 0f,
            ContinuousCd = true,
            ContactMonitor = true,
            MaxContactsReported = 8,
            CollisionLayer = 0b1000,  // layer 4 = player hands
            CollisionMask = 0b1001,   // world + hands; chest (layer 3) excluded both ways
        };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
        MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = HandSize } };
        StandardMaterial3D material = new() { AlbedoColor = color, Roughness = 0.5f };
        mesh.SetSurfaceOverrideMaterial(0, material);
        body.AddChild(mesh);
        AddChild(body);
        return body;
    }

    public override void _PhysicsProcess(double delta)
    {
        Chase(_left, _bodies[0], (float)delta);
        Chase(_right, _bodies[1], (float)delta);
    }

    private void Chase(XRController3D controller, RigidBody3D body, float delta)
    {
        if (body == null)
            return;
        if (BodyMath.ResetIfNonFinite(body, controller.GlobalTransform, "PhysicsHands"))
            return;
        int hand = _left == controller ? 0 : 1;
        Vector3 target = controller.GlobalPosition;
        Transform3D driveTarget = controller.GlobalTransform;
        bool armHeld = _arms != null && _arms.ConstrainHand(hand, ref driveTarget);

        bool lagging = body.GlobalPosition.DistanceTo(target) > GhostLagDistance;
        if (_ghosts[hand] != null)
        {
            _ghosts[hand].Visible = lagging;
            if (lagging)
                _ghosts[hand].GlobalTransform = controller.GlobalTransform;
        }

        if (!PropGrabber.IsGrabbing(controller))
        {
            // A hand held back by its own blocked arm is not stuck — snapping it into the
            // obstacle would just loop.
            if (body.GlobalPosition.DistanceTo(target) > StuckDistance && !armHeld)
                _stuckTimer[hand] += delta;
            else
                _stuckTimer[hand] = 0f;
            if (_stuckTimer[hand] > StuckTime)
            {
                _stuckTimer[hand] = 0f;
                body.GlobalTransform = controller.GlobalTransform;
                body.LinearVelocity = Vector3.Zero;
                GD.Print("PhysicsHands: hand stuck, snapped back");
                return;
            }

            if (body.GlobalPosition.DistanceTo(target) > SnapDistance)
            {
                body.GlobalTransform = controller.GlobalTransform;
                body.LinearVelocity = Vector3.Zero;
                return;
            }
        }
        Drive(body, driveTarget, delta);
    }

    // The hand chase itself — public so headless spikes drive their stand-ins with the real code.
    public static void Drive(RigidBody3D body, Transform3D target, float delta)
    {
        Vector3 desiredVelocity = (target.Origin - body.GlobalPosition) / delta;
        Vector3 dv = desiredVelocity - body.LinearVelocity;
        Vector3 force = dv / delta * body.Mass;
        if (force.Length() > HandMaxForce)
            force = force.Normalized() * HandMaxForce;
        body.LinearVelocity += force / body.Mass * delta;

        Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(),
            body.GlobalTransform.Basis.GetRotationQuaternion());
        body.AngularVelocity = (rotErr / delta).LimitLength(HandMaxSpin);
    }
}