Initial commit: VR Messs workspace (grab v5.3.1, body-physics research)
This commit is contained in:
Foxyz1302 committed
commit
5d2c5bf265
57 files changed
+4990
No files matched your search
@@ -0,0 +1,225 @@
|
||||
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 static PropGrabber _instance;
|
||||
|
||||
private XRController3D _left;
|
||||
private XRController3D _right;
|
||||
|
||||
private readonly RigidBody3D[] _held = new RigidBody3D[2];
|
||||
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 uint _grabCount;
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
_instance = this;
|
||||
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||||
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
||||
}
|
||||
|
||||
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;
|
||||
HoldUpdate(_left, 0, dt);
|
||||
HoldUpdate(_right, 1, dt);
|
||||
}
|
||||
|
||||
private void UpdateHand(XRController3D controller, int hand)
|
||||
{
|
||||
bool grip = controller.GetFloat("grip") > GripThreshold;
|
||||
int other = 1 - hand;
|
||||
|
||||
if (grip && !_gripPrev[hand] && _held[hand] == null)
|
||||
{
|
||||
RigidBody3D prop = FindProp(controller, hand);
|
||||
RigidBody3D handBody = PhysicsHands.GetBody(controller);
|
||||
if (prop != null && handBody != null)
|
||||
{
|
||||
_held[hand] = prop;
|
||||
_grabSeq[hand] = _grabCount++;
|
||||
Vector3 pivot = PalmPoint(handBody, hand);
|
||||
_joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias);
|
||||
_localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot;
|
||||
GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})");
|
||||
}
|
||||
}
|
||||
|
||||
if (!grip && _held[hand] != null)
|
||||
{
|
||||
RigidBody3D prop = _held[hand];
|
||||
_held[hand] = null;
|
||||
DestroyJoint(hand);
|
||||
if (IsInstanceValid(prop))
|
||||
GD.Print($"PropGrabber: released '{prop.Name}' at {prop.LinearVelocity.Length():F2} m/s");
|
||||
}
|
||||
_gripPrev[hand] = grip;
|
||||
}
|
||||
|
||||
private void HoldUpdate(XRController3D controller, int hand, float delta)
|
||||
{
|
||||
RigidBody3D 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(RigidBody3D 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, RigidBody3D 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, RigidBody3D 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)
|
||||
{
|
||||
if (_joints[hand] != null && IsInstanceValid(_joints[hand]))
|
||||
_joints[hand].QueueFree();
|
||||
_joints[hand] = null;
|
||||
_breakTimer[hand] = 0f;
|
||||
}
|
||||
|
||||
private 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 RigidBody3D 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;
|
||||
return NearestProp(handBody.GlobalPosition, CenterGrabRadius);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user