Added implementation of two-handed grabbing (GodotVR#563)

This PR implements two-handed grabbing with optional snapping of the hand to the grab-points.
Hand targeting was introduced to support hand snapping.
Hand targeting also used in climbing.
Added a grab-point redirector to force the hand to snap to a different object (for handles).
Add scope to sniper.
Changed physics priority to be set in code, and for the correct priority to prevent hand-lag.

Co-authored-by: Bastiaan Olij <mux213@gmail.com>
Co-authored-by: Miodrag Sejic <56046022+DigitalN8m4r3@users.noreply.github.com>

VERSIONS.md

@@ -5,6 +5,8 @@
 - **minor-breakage** Added support for swapping held items between hands.
 - Added jog-in-place movement provider.
 - Added support for grappling on GridMap instances
+- **breakage** Added support for two-handed grabbing.
+- Added support for snapping hands to grab-points.
 
 # 4.2.1
 - Fixed snap-zones showing highlight when disabled.

addons/godot-xr-tools/functions/function_pickup.gd

@@ -402,7 +402,7 @@ func _pick_up_object(target: Node3D) -> void:
 	# Pick up our target. Note, target may do instant drop_and_free
 	picked_up_ranged = not _object_in_grab_area.has(target)
 	picked_up_object = target
-	target.pick_up(self, _controller)
+	target.pick_up(self)
 
 	# If object picked up then emit signal
 	if is_instance_valid(picked_up_object):

addons/godot-xr-tools/functions/movement_climb.gd

@@ -68,6 +68,12 @@ var _dominant : Node3D
 # Right controller
 @onready var _right_controller := XRHelpers.get_right_controller(self)
 
+# Left collision hand
+@onready var _left_hand := XRToolsHand.find_left(self)
+
+# Right collision hand
+@onready var _right_hand := XRToolsHand.find_right(self)
+
 # Left collision hand
 @onready var _left_collision_hand := XRToolsCollisionHand.find_left(self)
 
@@ -205,6 +211,8 @@ func _on_left_picked_up(what : Node3D) -> void:
 	# If collision hands present then target the handle
 	if _left_collision_hand:
 		_left_collision_hand.add_target_override(_left_handle, 0)
+	elif _left_hand:
+		_left_hand.add_target_override(_left_handle, 0)
 
 
 ## Handler for right controller picked up
@@ -225,13 +233,17 @@ func _on_right_picked_up(what : Node3D) -> void:
 	# If collision hands present then target the handle
 	if _right_collision_hand:
 		_right_collision_hand.add_target_override(_right_handle, 0)
+	elif _right_hand:
+		_right_hand.add_target_override(_right_handle, 0)
 
 
 ## Handler for left controller dropped
 func _on_left_dropped() -> void:
 	# If collision hands present then clear handle target
 	if _left_collision_hand:
 		_left_collision_hand.remove_target_override(_left_handle)
+	if _left_hand:
+		_left_hand.remove_target_override(_left_handle)
 
 	# Release handle and transfer dominance
 	_left_handle = null
@@ -243,6 +255,8 @@ func _on_right_dropped() -> void:
 	# If collision hands present then clear handle target
 	if _right_collision_hand:
 		_right_collision_hand.remove_target_override(_right_handle)
+	if _right_hand:
+		_right_hand.remove_target_override(_right_handle)
 
 	# Release handle and transfer dominance
 	_right_handle = null

addons/godot-xr-tools/hands/collision_hand.gd

@@ -79,8 +79,10 @@ func _ready():
 	if Engine.is_editor_hint():
 		return
 
-	# Disconnect from parent transform, as we move to it in the physics step
+	# Disconnect from parent transform as we move to it in the physics step,
+	# and boost the physics priority above any grab-drivers or hands.
 	top_level = true
+	process_physics_priority = -90
 
 	# Populate nodes
 	_controller = XRTools.find_xr_ancestor(self, "*", "XRController3D")
@@ -171,6 +173,7 @@ func _move_to_target():
 	# Orient the hand then move
 	global_transform.basis = _target.global_transform.basis
 	move_and_slide(_target.global_position - global_position)
+	force_update_transform()
 
 
 # This function inserts a target override into the overrides list by priority

addons/godot-xr-tools/hands/hand.gd

@@ -41,6 +41,9 @@ var _last_world_scale : float = 1.0
 var _controller : XRController3D
 
 ## Initial hand transform (from controller) - used for scaling hands
+var _initial_transform : Transform3D
+
+## Current hand transform (from controller) - after scale
 var _transform : Transform3D
 
 ## Hand mesh
@@ -64,6 +67,12 @@ var _force_grip := -1.0
 ## Force trigger value (< 0 for no force)
 var _force_trigger := -1.0
 
+# Sorted stack of TargetOverride
+var _target_overrides := []
+
+# Current target (controller or override)
+var _target : Node3D
+
 
 ## Pose-override class
 class PoseOverride:
@@ -83,6 +92,20 @@ class PoseOverride:
 		settings = s
 
 
+## Target-override class
+class TargetOverride:
+	## Target of the override
+	var target : Node3D
+
+	## Target priority
+	var priority : int
+
+	## Target-override constructor
+	func _init(t : Node3D, p : int):
+		target = t
+		priority = p
+
+
 # Add support for is_xr_class on XRTools classes
 func is_xr_class(name : String) -> bool:
 	return name == "XRToolsHand"
@@ -91,7 +114,15 @@ func is_xr_class(name : String) -> bool:
 ## Called when the node enters the scene tree for the first time.
 func _ready() -> void:
 	# Save the initial hand transform
-	_transform = transform
+	_initial_transform = transform
+	_transform = _initial_transform
+
+	# Disconnect from parent transform as we move to it in the physics step,
+	# and boost the physics priority after any grab-drivers but before other
+	# processing.
+	if not Engine.is_editor_hint():
+		top_level = true
+		process_physics_priority = -70
 
 	# Find our controller
 	_controller = XRTools.find_xr_ancestor(self, "*", "XRController3D")
@@ -105,20 +136,21 @@ func _ready() -> void:
 	_update_hand_blend_tree()
 	_update_hand_material_override()
 	_update_pose()
+	_update_target()
 
 
-## This method is called on every frame. It checks for world-scale changes and
-## scales itself causing the hand mesh and skeleton to scale appropriately.
-## It then reads the grip and trigger action values to animate the hand.
-func _process(_delta: float) -> void:
+## This method checks for world-scale changes and scales itself causing the
+## hand mesh and skeleton to scale appropriately. It then reads the grip and
+## trigger action values to animate the hand.
+func _physics_process(_delta: float) -> void:
 	# Do not run physics if in the editor
 	if Engine.is_editor_hint():
 		return
 
 	# Scale the hand mesh with the world scale.
 	if XRServer.world_scale != _last_world_scale:
 		_last_world_scale = XRServer.world_scale
-		transform = _transform.scaled(Vector3.ONE * _last_world_scale)
+		_transform = _initial_transform.scaled(Vector3.ONE * _last_world_scale)
 		emit_signal("hand_scale_changed", _last_world_scale)
 
 	# Animate the hand mesh with the controller inputs
@@ -133,6 +165,9 @@ func _process(_delta: float) -> void:
 		$AnimationTree.set("parameters/Grip/blend_amount", grip)
 		$AnimationTree.set("parameters/Trigger/blend_amount", trigger)
 
+	# Move to target
+	global_transform = _target.global_transform * _transform
+	force_update_transform()
 
 
 # This method verifies the hand has a valid configuration.
@@ -161,14 +196,32 @@ func _get_configuration_warnings() -> PackedStringArray:
 ## Find an [XRToolsHand] node.
 ##
 ## This function searches from the specified node for an [XRToolsHand] assuming
-## the node is a sibling of the hand under an [ARVRController].
+## the node is a sibling of the hand under an [XROrigin3D].
 static func find_instance(node : Node) -> XRToolsHand:
 	return XRTools.find_xr_child(
 		XRHelpers.get_xr_controller(node),
 		"*",
 		"XRToolsHand") as XRToolsHand
 
 
+## This function searches from the specified node for the left controller
+## [XRToolsHand] assuming the node is a sibling of the [XROrigin3D].
+static func find_left(node : Node) -> XRToolsHand:
+	return XRTools.find_xr_child(
+		XRHelpers.get_left_controller(node),
+		"*",
+		"XRToolsHand") as XRToolsHand
+
+
+## This function searches from the specified node for the right controller
+## [XRToolsHand] assuming the node is a sibling of the [XROrigin3D].
+static func find_right(node : Node) -> XRToolsHand:
+	return XRTools.find_xr_child(
+		XRHelpers.get_right_controller(node),
+		"*",
+		"XRToolsHand") as XRToolsHand
+
+
 ## Set the blend tree
 func set_hand_blend_tree(blend_tree : AnimationNodeBlendTree) -> void:
 	hand_blend_tree = blend_tree
@@ -227,6 +280,32 @@ func force_grip_trigger(grip : float = -1.0, trigger : float = -1.0) -> void:
 	if trigger >= 0.0: $AnimationTree.set("parameters/Trigger/blend_amount", trigger)
 
 
+## This function adds a target override. The collision hand will attempt to
+## move to the highest priority target, or the [XRController3D] if no override
+## is specified.
+func add_target_override(target : Node3D, priority : int) -> void:
+	# Remove any existing target override from this source
+	var modified := _remove_target_override(target)
+
+	# Insert the target override
+	_insert_target_override(target, priority)
+	modified = true
+
+	# Update the target
+	if modified:
+		_update_target()
+
+
+## This function remove a target override.
+func remove_target_override(target : Node3D) -> void:
+	# Remove the target override
+	var modified := _remove_target_override(target)
+
+	# Update the pose
+	if modified:
+		_update_target()
+
+
 func _update_hand_blend_tree() -> void:
 	# As we're going to make modifications to our animation tree, we need to do
 	# a deep copy, simply setting resource local to scene does not seem to be enough
@@ -329,6 +408,59 @@ func _remove_pose_override(who : Node) -> bool:
 	return modified
 
 
+# This function inserts a target override into the overrides list by priority
+# order.
+func _insert_target_override(target : Node3D, priority : int) -> void:
+	# Construct the target override
+	var override := TargetOverride.new(target, priority)
+
+	# Iterate over all target overrides in the list
+	for pos in _target_overrides.size():
+		# Get the target override
+		var o : TargetOverride = _target_overrides[pos]
+
+		# Insert as early as possible to not invalidate sorting
+		if o.priority <= priority:
+			_target_overrides.insert(pos, override)
+			return
+
+	# Insert at the end
+	_target_overrides.push_back(override)
+
+
+# This function removes a target from the overrides list
+func _remove_target_override(target : Node) -> bool:
+	var pos := 0
+	var length := _target_overrides.size()
+	var modified := false
+
+	# Iterate over all pose overrides in the list
+	while pos < length:
+		# Get the target override
+		var o : TargetOverride = _target_overrides[pos]
+
+		# Check for a match
+		if o.target == target:
+			# Remove the override
+			_target_overrides.remove_at(pos)
+			modified = true
+			length -= 1
+		else:
+			# Advance down the list
+			pos += 1
+
+	# Return the modified indicator
+	return modified
+
+
+# This function updates the target for hand movement.
+func _update_target() -> void:
+	if _target_overrides.size():
+		_target = _target_overrides[0].target
+	else:
+		_target = get_parent()
+
+
 static func _find_child(node : Node, type : String) -> Node:
 	# Iterate through all children
 	for child in node.get_children():

addons/godot-xr-tools/hands/scenes/collision/collision_hand_left.tscn

@@ -6,7 +6,6 @@
 size = Vector3(0.045, 0.075, 0.1)
 
 [node name="CollisionHandLeft" type="AnimatableBody3D"]
-process_priority = -90
 collision_layer = 131072
 collision_mask = 327711
 sync_to_physics = false

addons/godot-xr-tools/hands/scenes/collision/collision_hand_right.tscn

@@ -6,7 +6,6 @@
 size = Vector3(0.045, 0.075, 0.1)
 
 [node name="CollisionHandRight" type="AnimatableBody3D"]
-process_priority = -90
 collision_layer = 131072
 collision_mask = 327711
 sync_to_physics = false

addons/godot-xr-tools/interactables/interactable_handle.gd

@@ -47,20 +47,20 @@ func _ready() -> void:
 # Called on every frame when the handle is held to check for snapping
 func _process(_delta: float) -> void:
 	# Skip if not picked up
-	if !picked_up_by:
+	if not is_picked_up():
 		return
 
 	# If too far from the origin then drop the handle
 	var origin_pos = handle_origin.global_transform.origin
 	var handle_pos = global_transform.origin
 	if handle_pos.distance_to(origin_pos) > snap_distance:
-		picked_up_by.drop_object()
+		drop()
 
 
 # Called when the handle is picked up
-func pick_up(by, with_controller) -> void:
+func pick_up(by) -> void:
 	# Call the base-class to perform the pickup
-	super(by, with_controller)
+	super(by)
 
 	# Enable the process function while held
 	set_process(true)

addons/godot-xr-tools/objects/climbable.gd

@@ -40,7 +40,7 @@ func request_highlight(_from, _on) -> void:
 	pass
 
 # Called by XRToolsFunctionPickup when this is picked up by a controller
-func pick_up(by: Node3D, _with_controller: XRController3D) -> void:
+func pick_up(by: Node3D) -> void:
 	# Get the ID to save the grab handle under
 	var id = by.get_instance_id()