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

scripts/PhysicsHands.cs

@@ -0,0 +1,138 @@
+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 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");
+        _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 = 1,
+            CollisionMask = 1,
+        };
+        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;
+        int hand = _left == controller ? 0 : 1;
+        Vector3 target = controller.GlobalPosition;
+
+        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))
+        {
+            if (body.GlobalPosition.DistanceTo(target) > StuckDistance)
+                _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;
+            }
+        }
+        Vector3 desiredVelocity = (target - 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;
+
+        Basis deltaBasis = controller.GlobalTransform.Basis * body.GlobalTransform.Basis.Inverse();
+        Quaternion q = deltaBasis.GetRotationQuaternion();
+        Vector3 spin = q.GetAxis() * (q.GetAngle() / delta);
+        if (spin.Length() > HandMaxSpin)
+            spin = spin.Normalized() * HandMaxSpin;
+        body.AngularVelocity = spin;
+    }
+}