VR-Mess/scripts/PropGrabber.cs

using Godot;

public partial class PropGrabber : Node
{
    private const float GripThreshold = 0.6f;
    private const float PalmGrabOffset = 0.045f;
    private const float GrabRadius = 0.16f;
    private const float CenterGrabRadius = 0.12f;
    private const float RigidWeldBias = 0f;
    private const float BreakDistance = 0.25f;
    private const float BreakTime = 0.25f;
    private const float BreakDistanceBoth = 0.4f;
    private const float GripRipDistance = 0.8f;
    private const float WorldGrabRadius = 0.07f;   // palm sphere for grabbing static world (ledges, bars, holds)
    public const string NoGrabGroup = "no_grab";   // static bodies that can't be grabbed (the floor)

    private static PropGrabber _instance;

    private XRController3D _left;
    private XRController3D _right;
    private PhysicsArms _arms;

    private readonly PhysicsBody3D[] _held = new PhysicsBody3D[2];  // a prop, or static world (climbing)
    private readonly ConeTwistJoint3D[] _joints = new ConeTwistJoint3D[2];
    private readonly Vector3[] _localAnchor = new Vector3[2];
    private readonly uint[] _grabSeq = new uint[2];
    private readonly float[] _breakTimer = new float[2];
    private readonly bool[] _gripPrev = new bool[2];
    private readonly SoftGrab[] _easing = new SoftGrab[2];   // grabbed, being eased into the hand (no weld yet)
    private uint _grabCount;

    public override void _Ready()
    {
        _instance = this;
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
        _arms = GetNodeOrNull<PhysicsArms>("../PhysicsArms");
    }

    public static bool IsGrabbing(XRController3D controller) =>
        _instance != null &&
        ((_instance._left == controller && _instance._held[0] != null) ||
         (_instance._right == controller && _instance._held[1] != null));

    public override void _Process(double delta)
    {
        UpdateHand(_left, 0);
        UpdateHand(_right, 1);
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        for (int hand = 0; hand < 2; hand++)
        {
            SoftGrab ease = _easing[hand];
            if (ease == null)
                continue;
            if (!IsInstanceValid(ease.Target) || !IsInstanceValid(ease.Hand))
                _easing[hand] = null;
            else if (ease.Step(dt))
            {
                _easing[hand] = null;
                Weld(hand == 0 ? _left : _right, hand, ease.Hand, ease.Target);
            }
        }
        HoldUpdate(_left, 0, dt);
        HoldUpdate(_right, 1, dt);
    }

    private void Weld(XRController3D controller, int hand, RigidBody3D handBody, PhysicsBody3D prop)
    {
        _held[hand] = prop;
        _grabSeq[hand] = _grabCount++;
        Vector3 pivot = PalmPoint(handBody, hand);
        _joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias);
        _localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot;
        _arms?.SetHeld(hand, prop as RigidBody3D);
        PlayerRig.SetHeld(controller, prop);
        string what = prop is RigidBody3D rb ? $"{rb.Mass:0.###} kg" : "static world";
        GD.Print($"PropGrabber: grabbed '{prop.Name}' ({what}, {(hand == 0 ? "left" : "right")}{(_held[1 - hand] == prop ? ", two-handed" : "")})");
    }

    private void UpdateHand(XRController3D controller, int hand)
    {
        bool grip = controller.GetFloat("grip") > GripThreshold;
        int other = 1 - hand;

        if (grip && !_gripPrev[hand] && _held[hand] == null && _easing[hand] == null)
        {
            PhysicsBody3D prop = FindProp(controller, hand);
            RigidBody3D handBody = PhysicsHands.GetBody(controller);
            if (prop != null && handBody != null)
                _easing[hand] = SoftGrab.Begin(handBody, hand, prop);   // welded once it's in the hand (_PhysicsProcess)
        }
        if (!grip)
            _easing[hand] = null;

        if (!grip && _held[hand] != null)
        {
            PhysicsBody3D prop = _held[hand];
            _held[hand] = null;
            DestroyJoint(hand);
            if (prop is RigidBody3D released && IsInstanceValid(released))
                GD.Print($"PropGrabber: released '{released.Name}' at {released.LinearVelocity.Length():F2} m/s");
        }
        _gripPrev[hand] = grip;
    }

    private void HoldUpdate(XRController3D controller, int hand, float delta)
    {
        PhysicsBody3D body = _held[hand];
        if (body == null)
            return;
        if (!IsInstanceValid(body))
        {
            _held[hand] = null;
            DestroyJoint(hand);
            return;
        }

        RigidBody3D handBody = PhysicsHands.GetBody(controller);
        ConeTwistJoint3D joint = _joints[hand];
        if (handBody == null || !IsInstanceValid(handBody) || joint == null || !IsInstanceValid(joint))
        {
            _held[hand] = null;
            DestroyJoint(hand);
            return;
        }

        if (handBody.GlobalPosition.DistanceTo(controller.GlobalPosition) > PhysicsHands.SnapDistance)
        {
            BreakGrip(hand, body, "grip dragged off");
            return;
        }

        float separation = (body.GlobalTransform * _localAnchor[hand]).DistanceTo(PalmPoint(handBody, hand));

        int other = 1 - hand;
        bool twoHanded = _held[other] == body && IsInstanceValid(_held[other]) &&
                         _joints[other] != null && IsInstanceValid(_joints[other]);
        if (twoHanded)
        {
            RigidBody3D otherHandBody = PhysicsHands.GetBody(other == 0 ? _left : _right);
            float otherSeparation = otherHandBody == null
                ? 0f
                : (body.GlobalTransform * _localAnchor[other]).DistanceTo(PalmPoint(otherHandBody, other));
            if (separation > BreakDistanceBoth && otherSeparation > BreakDistanceBoth)
            {
                BreakGrip(hand, body, $"both hands stretched ({separation:0.###} m)");
                BreakGrip(other, body, "both hands stretched");
                return;
            }
        }

        if (separation > GripRipDistance)
        {
            BreakGrip(hand, body, $"ripped ({separation:0.###} m)");
            return;
        }

        if (separation > BreakDistance)
            _breakTimer[hand] += delta;
        else
            _breakTimer[hand] = 0f;
        if (_breakTimer[hand] > BreakTime)
        {
            BreakGrip(hand, body, $"stretched ({separation:0.###} m for {BreakTime:0.#} s)");
            return;
        }
    }

    private int PrimaryHand(PhysicsBody3D body)
    {
        int primary = -1;
        for (int hand = 0; hand < 2; hand++)
        {
            if (_held[hand] == body && (primary == -1 || _grabSeq[hand] < _grabSeq[primary]))
                primary = hand;
        }
        return primary;
    }

    private ConeTwistJoint3D MakeWeld(RigidBody3D handBody, PhysicsBody3D prop, Vector3 pivot, float bias)
    {
        ConeTwistJoint3D weld = new()
        {
            ExcludeNodesFromCollision = true,
            Position = pivot,
            NodeA = handBody.GetPath(),
            NodeB = prop.GetPath(),
            SwingSpan = 0f,
            TwistSpan = 0f,
            Bias = bias,
        };
        AddChild(weld);
        return weld;
    }

    private void BreakGrip(int hand, PhysicsBody3D body, string reason)
    {
        if (_held[hand] != body)
            return;
        _held[hand] = null;
        DestroyJoint(hand);
        GD.Print($"PropGrabber: grip broke on '{body.Name}' ({reason})");
    }

    private void DestroyJoint(int hand)
    {
        _arms?.SetHeld(hand, null);
        PlayerRig.SetHeld(hand == 0 ? _left : _right, null);
        if (_joints[hand] != null && IsInstanceValid(_joints[hand]))
            _joints[hand].QueueFree();
        _joints[hand] = null;
        _breakTimer[hand] = 0f;
    }

    public static Vector3 PalmPoint(RigidBody3D handBody, int hand) =>
        handBody.GlobalPosition + handBody.GlobalTransform.Basis * new Vector3(PalmOffset(hand), 0f, 0f);

    private static float PalmOffset(int hand) => hand == 0 ? PalmGrabOffset : -PalmGrabOffset;

    private PhysicsBody3D FindProp(XRController3D controller, int hand)
    {
        RigidBody3D handBody = PhysicsHands.GetBody(controller);
        if (handBody == null)
            return null;
        Vector3 palm = PalmPoint(handBody, hand);
        RigidBody3D nearest = NearestProp(palm, GrabRadius);
        if (nearest != null)
            return nearest;
        nearest = NearestProp(handBody.GlobalPosition, CenterGrabRadius);
        if (nearest != null)
            return nearest;
        // Big props (planks, seesaw, boards): their origin is far from the hand — use the surface.
        World3D world = handBody.GetWorld3D();
        return BodyUnderPalm(palm, world) ?? BodyUnderPalm(handBody.GlobalPosition, world);
    }

    // The body whose surface is near the point: a "prop" first, else static world (climbing).
    public static PhysicsBody3D BodyUnderPalm(Vector3 point, World3D world)
    {
        PhysicsShapeQueryParameters3D query = new()
        {
            Shape = new SphereShape3D { Radius = WorldGrabRadius },
            Transform = new Transform3D(Basis.Identity, point),
            CollisionMask = 1,
        };
        StaticBody3D found = null;
        foreach (Godot.Collections.Dictionary hit in world.DirectSpaceState.IntersectShape(query, 8))
        {
            GodotObject o = GodotObject.InstanceFromId((ulong)hit["collider_id"]);
            if (o is RigidBody3D prop && prop.IsInGroup("prop"))
                return prop;
            if (o is StaticBody3D body && !body.IsInGroup(NoGrabGroup))
                found ??= body;
        }
        return found;
    }

    // Climbing: any static body near the palm (except the "no_grab" group) can be held.
    // NOTE (Box3D wrapper): shape-query results carry a null "collider" — resolve "collider_id".
    public static StaticBody3D WorldUnderPalm(Vector3 palm, World3D world)
    {
        PhysicsShapeQueryParameters3D query = new()
        {
            Shape = new SphereShape3D { Radius = WorldGrabRadius },
            Transform = new Transform3D(Basis.Identity, palm),
            CollisionMask = 1,
        };
        foreach (Godot.Collections.Dictionary hit in world.DirectSpaceState.IntersectShape(query, 8))
        {
            if (GodotObject.InstanceFromId((ulong)hit["collider_id"]) is StaticBody3D body && !body.IsInGroup(NoGrabGroup))
                return body;
        }
        return null;
    }

    private RigidBody3D NearestProp(Vector3 point, float radius)
    {
        RigidBody3D nearest = null;
        float nearestDist = radius;
        foreach (Node node in GetTree().GetNodesInGroup("prop"))
        {
            if (node is not RigidBody3D prop)
                continue;
            float dist = prop.GlobalPosition.DistanceTo(point);
            if (dist < nearestDist)
            {
                nearestDist = dist;
                nearest = prop;
            }
        }
        return nearest;
    }
}