Initial commit: VR Messs workspace (grab v5.3.1, body-physics research)

scripts/AnchorMoveMode.cs

@@ -0,0 +1,95 @@
+using Godot;
+
+public partial class AnchorMoveMode : Node
+{
+    private const float GripThreshold = 0.6f;
+
+    private XROrigin3D _origin;
+    private XRCamera3D _camera;
+    private XRController3D _left;
+    private XRController3D _right;
+
+    private bool _wasTwoHand;
+    private Vector3 _leftPrev;
+    private Vector3 _rightPrev;
+    private Vector3 _twoHandMidPrev;
+    private float _twoHandYawPrev;
+
+    public override void _Ready()
+    {
+        _origin = GetNode<XROrigin3D>("../XROrigin3D");
+        _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
+        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
+        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
+    }
+
+    public override void _Process(double delta)
+    {
+        bool leftGrip = _left.GetFloat("grip") > GripThreshold && !PropGrabber.IsGrabbing(_left);
+        bool rightGrip = _right.GetFloat("grip") > GripThreshold && !PropGrabber.IsGrabbing(_right);
+
+        if (leftGrip && rightGrip && !WorldPanel.IsBlocking(_left) && !WorldPanel.IsBlocking(_right))
+            TwoHandGrab();
+        else
+        {
+            _wasTwoHand = false;
+            if (leftGrip && !WorldPanel.IsBlocking(_left))
+                GrabMove(_left, ref _leftPrev);
+            else
+                _leftPrev = _left.Position;
+            if (rightGrip && !WorldPanel.IsBlocking(_right))
+                GrabMove(_right, ref _rightPrev);
+            else
+                _rightPrev = _right.Position;
+        }
+    }
+
+    private void TwoHandGrab()
+    {
+        Vector3 mid = Midpoint();
+        float yaw = HandsYaw();
+
+        if (!_wasTwoHand)
+        {
+            _wasTwoHand = true;
+            _twoHandMidPrev = mid;
+            _twoHandYawPrev = yaw;
+            return;
+        }
+
+        Vector3 deltaWorld = _origin.GlobalTransform.Basis * (mid - _twoHandMidPrev);
+        float yawDelta = _twoHandYawPrev - yaw;
+        if (deltaWorld.LengthSquared() > 0.000001f || Mathf.Abs(yawDelta) > 0.000001f)
+        {
+            Vector3 hxz = new Vector3(_camera.GlobalPosition.X, 0f, _camera.GlobalPosition.Z);
+            Transform3D turn = new Transform3D(Basis.Identity, hxz)
+                * new Transform3D(Basis.FromEuler(new Vector3(0, yawDelta, 0)), Vector3.Zero)
+                * new Transform3D(Basis.Identity, -hxz);
+            _origin.GlobalTransform = new Transform3D(Basis.Identity, -deltaWorld) * turn * _origin.GlobalTransform;
+        }
+        _twoHandMidPrev = mid;
+        _twoHandYawPrev = yaw;
+    }
+
+    private void GrabMove(XRController3D controller, ref Vector3 prev)
+    {
+        Vector3 pos = controller.Position;
+        Vector3 delta = pos - prev;
+        prev = pos;
+        if (delta.LengthSquared() == 0f)
+            return;
+        Vector3 deltaWorld = _origin.GlobalTransform.Basis * delta;
+        _origin.GlobalTransform = new Transform3D(Basis.Identity, -deltaWorld) * _origin.GlobalTransform;
+    }
+
+    private Vector3 Midpoint() => (_left.Position + _right.Position) * 0.5f;
+
+    private float HandsYaw()
+    {
+        Vector3 v = _right.Position - _left.Position;
+        v.Y = 0f;
+        if (v.LengthSquared() < 0.0001f)
+            return _twoHandYawPrev;
+        return Mathf.Atan2(v.X, v.Z);
+    }
+}